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1.  INTRODUCTION 


1.1  Simulation  Overview 

The  Computer  Engineering  Research  Laboratory  at  the  Georgia  Institute  of  Technology,  under  con¬ 
tract  to  the  United  States  Army  Strategic  Defense  Command,  is  developing  special  purpose  parallel 
computers  for  hardware-in-the-loop  simulation  and  testing  of  kinetic  energy  weapons  (KEW)  sys¬ 
tems  and  components.  Of  primary  interest  is  the  ability  to  test  guidance,  navigation  and  control 
(GN&C)  algorithms.  This  paper  presents  details  on  a  hardware-in-the-loop  test  of  a  Honeywell 
(Sandia)  S5,  GN&C  processor.  The  simulation  emphasizes  object  processing  and  assumes  informa¬ 
tion  from  the  seeker  sensor  has  been  processed  to  produce  object  centroids.  The  objectives  of  the 
simulation  were  to: 

1.  Demonstrate  hardware-in-the-loop  testing  with  real  GN&C  technology. 

2.  Demonstrate  real-time  simulation  capability  using  ADA  software. 

3.  Assess  the  performance  of  the  simulation  processor  relative  to  the  GN&C  processor. 

4.  Demonstrate  the  effectiveness  of  the  SUN  386i  as  a  user  interface  (host  computer) 
to  the  simulation  machine. 

This  report  presents  the  simulation  development  in  Section  2,  the  simulation  model  in  Section  3, 
simulation  hardware  in  Section  4,  simulation  software  in  Section  6,  the  GN&C  processor  in  Section 
7,  and  the  HWIL  test  and  conclusions  in  Section  8.  Source  code  for  the  test  is  given  in  the  Appendix, 
Section  9. 
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2.  DEVELOPMENT  STAGES 

This  simulation  required  the  development  of  hardware  and  software.  These  two  were  developed  in 
stages.  The  overall  simulation  is  rudimentary  since  it  is  the  first  step  in  a  much  more  involved  pro¬ 
gram.  The  objectives  were  to  get  all  the  hardware  units  running  in  a  real  HWIL  configuration.  En¬ 
hancements  will  be  added  as  this  project  progresses. 

2.1  The  simulation 

The  simulation  scenario  was  a  single  exo-atmospheric  interceptor  in  the  final  stages  of  selecting  an 
incoming  ballistic  missile,  tracking  it,  adjusting  course  and  destroying  it  by  a  kinetic  energy  impact. 

The  simulation  begins  with  the  interceptor  using  an  infrared  focal  plane  array  (FPA)  sensor  at  100 
frames  per  second  to  detect  possible  targets.  Initially  no  targets  are  visible,  so  the  interceptor  pro¬ 
ceeds  on  the  current  trajectory  until  a  target  is  visible.  When  targets  become  visible  for  several 
frames  the  interceptor  begins  calculating  the  trajectory  of  the  targets.  At  some  point,  a  particular 
target  is  selected,  and  the  interceptor  adjusts  its  course  in  an  attempt  to  collide  with  the  target. 

During  the  simulation,  two  graphics  windows  on  the  host  are  presented  for  observation.  The  first 
window  illustrates  the  view  of  the  target  space  as  seen  by  the  FPA.  The  second  window  is  the  view 
of  an  observer  sitting  at  a  some  point  in  space.  The  observation  location  can  be  selected  in  a  configu¬ 
ration  file.  Information  on  range  to  the  nearest  target,  and  which  frame  is  currently  being  displayed 
on  the  host  are  also  shown  by  the  host. 

The  course  corrections  continue  until  the  KEW  interceptor  ’’hits”  the  target,  or  passes  by.  The  simu¬ 
lation  returns  a  nearest  miss  distance.  The  nearest  miss  distance  may  actually  be  a  collision  if  the 
miss  distance  is  below  a  certain  adjustable  threshold.  If  the  simulation  detects  a  collision,  suitable 
display  effects  on  the  graphics  output  indicate  the  collision  event. 
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Figure  1.  shows  the 
graphics  screen  on  the 
Sun  host  computer  at  an 
early  stage  in  the  simu¬ 
lation.  In  the  World 
View  window,  the  tar¬ 
gets  are  on  the  left,  and 
the  KEW  interceptor  is 
on  the  right  The  FPA 
View  window  shows 
eight  targets  seen  as 
point  sources  by  the 
KEW  interceptor.  The 
windows  on  the  upper 
right  were  used  to  start 
the  simulation,  and  to 
take  a  snapshot  of  the 
screen.  Figure  2.  shows 
the  end  of  the  same  sim¬ 
ulation  The  course  cor¬ 
rections  performed  by 
the  interceptor  are  too 
small  to  appear  as  any 
deviation  from  a 
straight  line,  since  a 
pixel  on  the  display  ac¬ 
tually  covers  200x200 
meters.  The  center  to 
center  miss  distance  as 
shown  in  the  World 
View  is  0.24  meters, 
counting  as  a  hit  The 
”Non  Real-Time  Sim¬ 
ulation:  0.680  Sec 
Lag!”  indicates  that  the 
simulation  did  not  run 
in  real  time.  This  was 
caused  by  the  host 
which  for  this  simula¬ 
tion,  displayed  every 
frame,  slowing  the  sim¬ 
ulation. 
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Figure  1.  Beginning  of  Simulation 


Figure  2.  End  of  Simulation 
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2.2  Host  only 

The  simulation  was  initially  begun  on  a  Sun  386i.  This  was  convenient  since  the  Sun  386i  was  an 
available  platform  maintained  on  a  network  at  Georgia  Tech.  One  of  the  networked  Sun  386i’s  was 
also  the  eventual  host  for  the  PFP,  minimizing  the  problem  of  transporting  code  between  this  stage 
and  later  stages. 

C  was  chosen  as  the  development  language,  because  the  Sun  is  aUnix  platform  and  strongly  supports 
C  with  debuggers  and  windowing  libraries.  The  various  algorithms  that  were  used  had  to  be  col¬ 
lected  and  converted  to  C.  Some  of  this  code  was  originally  in  Occam,  the  Inmos  Transputer ’s  native 
language.  Other  pieces  had  been  developed  on  PCs  in  Turbo-Pascal.  These  languages  were  all  con¬ 
verted  to  separately  compiled  modules  in  C.  The  conversion  was  done  with  a  plan  to  eventually  run 
these  algorithms  on  a  parallel  machine.  This  required  the  modules  to  be  developed  in  such  a  way 
that  they  would  easily  map  to  one  processor  on  the  target  machine. 

Since  the  target  architecture  does  not  support  shared  memory,  all  shared  variables  must  be  passed 
as  messages  to  other  processors  that  use  the  data.  While  writing  the  modules,  the  code  was  devel¬ 
oped  with  carefully  constructed  global  structures.  These  structures  are  the  elements  that  eventually 
must  be  transmitted  over  the  communication  network  on  the  parallel  target  machine.  All  other  vari¬ 
ables  were  kept  local  to  the  module  which  used  them. 

2.3  PFP 

The  target  machine  is  a  parallel  computer  designated  as  a  Parallel  Function  Processor  (PFP)  (see 
Section  4).  The  C  code  was  modified  to  run  on  the  Parallel  Function  Processor  (PFP).  This  required 
replacing  all  of  the  global  data  structures  with  the  send()  and  receive()  calls  used  on  the  PFP.  In  con¬ 
junction  with  the  code  development,  crossbar  code  was  developed  to  link  the  sender  to  the  receiver 
at  the  appropriate  time.  This  has  to  be  done  carefully  as  there  is  no  consistency  checking  to  assure 
that  the  receiver  gets  data  from  the  desired  source.  An  incorrect  crossbar  program  will  simply  pass 
the  wrong  data  to  a  processor,  or  not  pass  data  to  it  at  all.  Reading  from  a  processor  that  is  not  sending 
will  cause  the  entire  crossbar  communication  network  to  deadlock.  Debugging  tools  to  handle  this 
situation  were  developed  in  conjunction  with  debugging  the  code. 

The  host  based  simulation  modeled  only  one  target  due  to  processing  speed  limitations.  Given  the 
parallel  architecture  and  higher  performance  of  the  PFP  nodes,  multiple  targets  could  be  modeled 
on  the  PFP  by  replicating  the  target  generation  module,  and  modifying  the  crossbar  code  to  deal  with 
multiple  targets.  The  simulation  continuously  passed  data  back  to  the  host,  but  this  quickly  became 
the  limitation  on  execution  time.  The  host  system  is  Unix  based,  and  is  therefore  notoriously  incon¬ 
sistent  in  how  fast  it  can  handle  data.  To  overcome  this  problem  while  still  sampling  as  much  data 
as  possible,  an  additional  buffer  module  was  added  to  the  simulation  which  collected  data  samples 
every  simulation  cycle  (100  Hz).  The  host  signalled  this  module  when  it  was  ready  to  receive  a  data 
sample.  Other  data  packets  were  lost.  To  allow  full  data  collection,  the  buffer  module  was  modified 
to  run  either  in  a  demand  mode  as  before,  or  to  force  every  N*  sample  to  be  passed  to  the  host,  delay¬ 
ing  the  simulation  until  the  host  was  ready  for  data. 

The  code  was  then  converted  on  a  module  by  module  basis  to  Ada.  Each  module  was  re-written 
in  Ada,  and  its  results  (with  the  rest  of  the  system  running  C  based  code)  were  compared  to  the  results 
of  the  C  version  of  that  module. 

2.4  HWIL 

The  final  stage  of  the  simulation  was  to  replace  the  module  simulating  the  object  tracking  with  an 
off  the  shelf  flight  processor.  This  processor  was  connected  in  place  of  one  of  the  modules  on  the 
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PFP.  A  compiler  could  not  be  obtained  for  the  flight  processor  due  to  licensing  delays.  The  solution 
to  this  was  to  send  the  C  code  for  the  module  to  the  vendor  for  the  flight  processor.  They  compiled 
the  code  and  returned  the  loadable  program.  Since  the  code  had  to  be  sent  back  and  forth  via  over¬ 
night  mail,  turnaround  time  was  approximately  one  week. 

A  problem  developed  with  the  data  ordering  in  the  structure  between  the  PFP  and  the  flight  proces¬ 
sor.  To  avoid  the  long  turnaround  delay  of  re-compiling  the  flight  processor  code,  this  was  solved 
by  inserting  an  additional  module  in  the  simulation  which  re-ordered  the  data  for  the  flight  proces¬ 
sor. 
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3.  SIMULATION  MODELS 


3.1  FPA  Model 

The  KEW’s  Focal  Plane  Array  (FPA)  is  a  fixed  position,  128x128  pixel,  infrared  detector.  A  full 
FPA  model  would  contain  optical  distortion,  detector  sensitivity,  A/D  quantization  error  and  other 
noise  sources.  For  the  purpose  of  this  simulation,  which  was  constructed  as  a  demonstration  of  the 
PFP’s  capability,  the  FPA  model  is  combined  with  the  signal  processing  to  produce  a  very  simple 
model. 

The  signal  processing  section  of  the  KEW’s  avionics  recognizes  non-overlapping  objects  in  the 
FPA’s  field  of  view,  and  returns  the  pixel  column  and  row  of  the  centroid  of  the  object  (xc,yc),  the 
pixel  column  and  row  of  the  intensity  centroid  (x^y^),  the  total  size  in  pixels  of  the  object  (Nc),  and 
the  total  intensity  of  the  object  (Ic).  These  are  calculated  using  the  following 

WV  i*N  M 

-2—  ,  yc  =  -~-  >  xd  =  — —  ,  y*  =  — — • 

N  N  Ic  *c 

For  this  simulation,  the  position  of  the  target  was  directly  mapped  into  the  pixel  cordinates  (xc,yc). 
The  total  number  of  pixels  (Nc)  was  calculated  based  on  size  and  range,  and  the  total  intensity  (Ic) 
was  calculated  based  on  range  and  target  intensity. 

3.2  Target  Model 

The  target  was  modeled  as  a  uniform  temperature  body,  resulting  in  the  area  centroid  being  identical 
to  the  intensity  centroid.  The  target  model  allowed  the  thermal  radiation  intensity  and  the  size  of 
the  target  to  be  set  on  a  per  target  basis,  allowing  multiple  size  and  intensity  targets  in  one  simulation. 

3.3  KEW  Model 

The  KEW  is  modeled  as  a  simple  rigid  body  with  an  orientation  vector  and  a  velocity  vector.  The 
orientation  vector  is  normal  to  the  FPA.  The  velocity  vector  determines  the  flight  path  of  the  KEW. 
Two  types  of  compensation  are  applied  to  the  KEW  during  the  homing  phase  of  the  flight.  The  first 
type  of  compensation  is  to  orient  the  KEW  such  that  the  designated  target  is  at  the  center  of  the  FPA. 
The  direction  of  rotation  is  proportional  to  the  location  of  the  target  in  the  FPA.  The  second  type 
of  compensation  is  the  divert  maneuver  of  the  KEW.  The  divert  is  proportional  to  the  predicted  ve¬ 
locity  of  the  target  on  the  FPA. 

3.4  Flight  Processor 

The  flight  processor  ran  a  tracking  filter  based  on  a  Kalman  filter^  This  is  only  a  small  portion  of 
what  an  actual  flight  processor  would  have  to  run,  but  was  sufficient  to  stress  the  example  flight  pro¬ 
cessor  we  were  using  for  this  demonstration. 
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4.  PARALLEL  FUNCTION  PROCESSOR 


4.1  System  Compents 


A  Parallel  Function  Processor  (PFP)  is  a  parallel  computing  array  consisting  of  the  following  com¬ 
ponents  (see  Figure  3.): 

Host  Computer  with  the  following  peripherals  - 


Terminal 

Printer 

Removable  Hard  Disk  Drive 

One  or  two  Crossbar  Switches  (16x16  array) 

Sequencer  (crossbar  switch  controller) 

PPEs  (32  processor  boards  per  crossbar) 

These  components  are  embedded  in  a  chassis  with  Multibus 
I  card  cages,  power  supplies  and  fans.  In  addition  there  are 
display  boards  to  indicate  the  crossbar  switch  pattern  and  the 
processor  status. 

The  PFP  can  be  expanded  as  shown  in  Figure  4.,  to  include 
64  PPEs.  In  Figure  4.,  the  components  which  have  been  add¬ 
ed  are  a  crossbar  switch,  sequencer  and  32  parallel  process¬ 
ing  elements.  This  system  is  essentially  a  duplicate  of  the 
components  in  Figure  3.,  except  for  the  Host  Computer. 

In  Figure  4.  there  are  actually  two  parallel  computing  arrays 
which  can  operate  independently  on  separate  problems,  or 
together  on  a  single  problem.  When  operating  together,  an 
Array  Interconnect  Module  is  inserted  into  a  PPE  port  to  pro¬ 
vide  a  connection  between  the  two  units.  More  than  one 
Array  Interconnect  Module  can  be  used,  but  each  connection 
causes  the  loss  of  two  PPEs.  For  higher  bandwidth  four  in¬ 
terconnection  modules  are  used  to  replace  four  PPEs,  pro¬ 
viding  two  links  between  the  PFP  units.  The  hardware  is 
packaged  in  three  19”  racks  except  for  the  host  and  printer. 

4.2  Host  Computer 

The  sequencer,  crossbar  switch  and  processors  all  reside  on 
a  common  computer  bus  (Intel  Multibus).  This  bus  is  con¬ 
trolled  by  a  host  computer  which  is  used  to  download  the  se¬ 
quencer,  crossbar  switch  and  processors  with  their  necessary 
microcode  or  program.  Previously  an  Intel  310  computer 
(80286  based)  was  used  as  the  host  for  all  code  development. 

A  SUN  Microsystems  386i  workstation  is  currently  used  as 
the  host  computer.  The  SUN  386i  host  is  connected  to  the 
PFP  Multibus  using  a  BIT3  card  set  which  converts  between 
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the  Sun’s  internal  PC-AT  style  bus  and  the  Intel  Multibus. 

The  SUN  host  is  used  to  compile,  link  and  load  programs  to  the  processing  elements,  the  crossbar 
sequencer  and  the  crossbar  memory.  The  SUN  386i  host  provides  many  features  not  available  on 
the  Intel  310  host  such  as  graphical  user  interface  (block  diagram  editor),  remote  login  capability, 
and  an  integrated  parallel  programming  framework  for  the  development  of  Ada  programs. 


4.3  Sequencer 

The  Sequencer  is  the  master  state  machine  of  the  computer.  The  responsibility  of  the  sequencer  is 
to  clock  the  crossbar  switch  for  the  appropriate  communication  configuration  and  to  control  the 
handshake  lines  of  each  processor.  The  microcode  of  the  sequencer,  stored  in  a  96  x  1024  bit 
memory,  defines  the  sending  and  receiving  processors  and  the  next  cycle  address  where  the  sequenc¬ 
er  and  crossbar  memory  can  retrieve  information.  The  next  cycle  address  allows  both  the  sequencer 
and  crossbar  to  run  in  a  pipelined  mode,  removing  memory  read  time  from  execution  delay. 

Once  the  sequencer  is  loaded  with  the  microcode  for  a  problem,  it  is  started  by  writing  a  control  word 
to  the  I/O  control  port  of  the  sequencer.  Immediately  the  sequencer  will  load  the  information  for 
the  first  cycle  of  the  communication  configuration.  The  sequencer  will  check  the  handshake  lines 
of  the  processors  and  wait  till  all  the  senders  and  receivers,  for  that  particular  cycle,  are  ready.  When 
this  condition  is  met,  the  sequencer  transmits  handshake  signals  to  the  active  processors  to  latch  data 
in  and  clears  handshake  lines  for  the  next  cycle.  When  the  sequencer  is  started,  the  crossbar  switch 
is  also  enabled  and  the  switch  configuration  for  the  first  cycle  is  loaded.  At  each  cycle  the  sequencer 
uses  the  next  sequencer  address  to  fill  the  pipeline  for  the  next  cycle.  The  sequencer  also  transmits 
the  next  crossbar  address  so  the  crossbar  pipeline  will  also  be  ready  for  the  next  cycle.  Each  cycle 
is  synchronized  by  waiting  for  all  active  sending  and  receiving  processors  to  become  ready.  The 
sequencer  will  continue  to  run  until  it  is  reset  For  most  problems,  the  sequencer  will  stop  at  a  certain 
cycle  when  the  processors  finish  their  tasks  and  no  longer  generate  handshake  signals  to  the  sequen¬ 
cer. 

4.4  Crossbar  Switch 

The  crossbar  switch  makes  up  the  second  component  of  the  communication  network.  It  consists  of 
four  circuit  boards,  where  each  board  is  a  4x4, 16  bit  bi-directional  switch.  The  crossbar  switch  is 
physically  arranged  so  that  there  are  horizontal  ”x”  buses  and  vertical  ”y”  buses.  There  are  sixteen 
”x”  buses  connected  to  sixteen  ”y”  buses.  Each  circuit  board  has  memory  to  store  control  bits.  The 
memory  stores  32  x  1024  bits  of  microcode,  which  defines  the  control  for  each  switch  on  the  circuit 
board.  There  are  three  possible  states  for  each  switch:  ”x”  to  ”y”  transfer,  ”y”  to  ”x”  transfer  and 
tri-state  for  no  transfer.  Each  circuit  board  depends  on  the  sequencer  to  provide  the  next  crossbar 
address  to  retrieve  control  bit  information  from  the  microcode  memory.  Since  the  microcode  for 
each  circuit  board  is  stored  in  the  same  order,  the  sequencer  only  provides  one  next  crossbar  address 
to  all  four  circuit  boards. 

The  crossbar  switch  is  enabled  by  the  sequencer  when  the  proper  control  word  is  written  to  the  I/O 
control  port  of  the  sequencer.  Once  enabled,  the  crossbar  switch  will  load  the  switch  configuration 
for  the  first  cycle.  Since  the  sequencer  is  the  master  state  machine,  it  will  clock  the  crossbar  to  use 
the  next  crossbar  address  to  load  information.  Using  the  next  crossbar  address,  the  crossbar  switch 
will  set  the  switch  configuration  before  the  sequencer  begins  checking  the  handshake  lines  of  the 
processors.  The  crossbar  switch  maintains  a  particular  switch  setting  until  the  sequencer  clocks  it 
or  the  crossbar  switch  is  reset. 
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4.5  Parallel  Processing  Element 

A  Parallel  Processing  Element  (PPE)  must  meet  two  criteria  to  be  used  in  the  PFP.  First,  the  proces¬ 
sor  must  meet  the  Multibus  standard  to  provide  power  and  an  interface  for  the  host  processor.  Sec¬ 
ond,  the  processor  must  meet  the  crossbar  interface  standard  (proprietary  standard  for  the  computer) 
to  provide  handshake  signals  to  the  sequencer  and  a  data  bus  to  the  crossbar  switch. 

A  custom  floating  point  processor  (GT-FPP  or  FPP)  designed  at  Georgia  Tech  was  used  for  this  sim¬ 
ulation.  The  FPP  processor  is  a  custom  processor  board  designed  at  CERL  for  use  in  the  PFP.  The 
FPP  is  a  three  address  machine,  using  the  AMD29325  32  bit  floating  point  ALU,  in  conjunction  with 
dual  port  memory,  and  an  AMD  2910  sequencer. 
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5.  SIMULATION  INTERFACE 


5.1  Host 

A  Sun  386i  was  used  as  a  host  computer  for  the  PFP.  The  Sun  386i  uses  a  25  MHz  80386  CPU  with 
the  80387  floating  point  co-processor,  12  MB  main  memory,  a  307MB  Hard  disk,  and  a  1152x900 
pixel  color  monitor.  The  host  system  supports: 

1 .  FPP  processor  program  compilation, 

2.  crossbar  sequencer  program  compilation, 

3.  FPP  and  crossbar  sequencer  program  loading, 

4.  all  program  storage,  either  online  (hard  disk)  or  offline  (floppy  disk  or  tape), 

5.  debugging  access, 

6.  network  access  to  the  PFP, 

7.  graphical  output  display, 

8.  windowed  user  interface,  and 

9.  data  and  simulation  result  collection. 

5.1.1  Host  to  Processor  Element  program  loading 

The  processor  boards  are  accessible  from  the  host  via  a  multibus  repeater  array.  The  processor  se¬ 
lects  a  specific  multibus  rack  within  the  PFP.  Within  a  given  rack,  each  processor  is  assigned  a  spe¬ 
cific  64kB  memory  range.  To  select  a  specific  processor,  this  memory  range  is  mapped  into  the  host 
memory  map.  The  device  drivers  on  the  host  are  written  such  that  this  is  handled  automatically 
whenever  a  specific  processor  is  accessed.  Once  a  processor  is  mapped  into  the  host  memory  space, 
the  code  is  loaded  by  direct  memory  access  from  the  host. 

For  the  FPP,  the  memory  range  contains  the  program  memory,  a  control  register,  and  the  two  FIFOs 
for  communication  with  the  processor.  Since  the  FPP  employs  separate  address  and  data  spaces, 
and  the  data  memory  is  not  accessible  by  the  host,  it  is  necessary  to  first  load  and  run  a  data  memory 
loader.  The  data  memory  loader  takes  data  from  the  host  to  FPP  communication  FIFO  and  stores 
it  in  data  memory.  The  actual  program  code  is  then  loaded  to  the  processor’s  instruction  memory. 

The  control  register  allows  the  host  to  start  and  stop  the  FPP,  and  to  check  the  status  of  the  host  to 
FPP  and  FPP  to  host  FIFOs.  The  status  flags  identify  when  the  FPP  to  host  FIFO  is  empty,  and  when 
the  host  to  FPP  is  full.  The  intent  is  to  allow  the  host  to  easily  identify  if  an  FPP  has  data  for  the 
host,  and  to  determine  if  the  FPP’s  FIFO  has  room  for  host  to  FPP  data. 

5.1.2  Host  to  Processor  Element  runtime  communications 

The  host  to  processor  communication  mechanism  was  designed  for  a  simple  host  that  acted  only  as 
a  loader.  With  the  Sun  386i’s  1 152x900  color  graphics  monitor,  the  host  can  play  an  important  part 
in  a  simulation.  This  brings  up  the  problem  of  transferring  data  from  the  processors  to  the  host  in 
a  system  that  was  designed  to  use  the  host  strictly  for  loading  programs.  Since  the  host  can  read  as 
well  as  write  processor  memory,  a  bi-directional  communication  capability  exists.  The  FPP  was 
built  to  capitalize  on  this  by  using  two  memory  mapped  FIFOs  to  allow  buffered  messages  to  be 
passed  between  the  host  and  the  processor. 

No  mechanism  for  passing  interrupts  between  the  host  and  processor  exist,  requiring  a  polling  mode 
of  data  transfer  between  the  two.  The  host  is  the  master  in  any  transfers,  since  the  PPE  does  not  have 
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any  method  of  initiating  transfers  to  the  host.  In  sending  from  a  processor  to  the  host,  the  processor 
loads  data  into  a  processor  to  host  FIFO.  The  host  selects  the  processor  then  polls  the  status  register 
to  determine  if  there  is  new  data  present  in  the  FIFO,  and  if  so  can  read  the  data  from  the  FIFO.  Host 
to  processor  communications  do  not  require  polling  by  the  host.  The  host  selects  the  target  proces¬ 
sor,  checks  that  the  FIFO  is  not  full,  and  writes  the  data  to  the  host  to  processor  FIFO. 

A  Unix  device  special  file  acts  as  the  access  point  for  the  two  FIFOs  on  each  processor.  This  allows 
the  standard  unix  syntax  of  opening,  reading  and  writing  to  be  used  for  accessing  each  processor. 
The  device  driver  handles  all  of  the  FIFO  status  checking,  and  uses  the  standard  Unix  error  handling 
routines.  Programs  on  the  host  can  then  use  all  of  the  Unix  I/O  libraries  for  accessing  the  FPPs. 

5.1.3  Host  Network  Interface 

The  host  is  connected  to  the  local  ethemet.  This  allows  the  host  to  use  the  full  range  of  network  fea¬ 
tures  available  on  modem  Unix  implementations.  Remote  tape  drives  are  used  for  backing  up  the 
data  files  on  the  host,  remote  access  to  laser  printer  for  printouts,  and  file  sharing  via  NFS  to  remote 
servers.  The  most  important  feature  of  the  network  access  is  the  ability  to  work  on  the  PFP  from 
remote  hosts.  This  allowed  the  simulation  to  be  developed  by  several  people  from  multiple  worksta¬ 
tions.  This  was  important  since  the  group  supporting  the  software  tools  reside  in  a  separate  building, 
yet  they  could  work  on  the  system  over  the  network  from  their  facility.  The  simulation  produced 
graphical  output  that  was  developed  under  Sun  Microsystem’s  Sun  View  which  does  not  allow  net¬ 
worked  graphics  windows.  This  required  the  simulation  to  be  run  on  the  host  computer  when  graphi¬ 
cal  output  was  desired.  Future  use  of  the  XI 1  window  system  will  allow  remote  graphical  display. 
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6.  SIMULATION  SOFTWARE 

The  parallel  architecture  of  the  PFP  and  the  use  of  custom  hardware  designs  requires  custom  soft¬ 
ware  tools  to  program  and  debug  any  simulation.  The  software  tools  for  programming  the  PFP  are 
being  developed  at  Georgia  Tech  with  the  aid  of  the  School  of  Information  and  Computer  Science. 

6.1  Support  Software 

Support  software  under  development  at  Georgia  Tech  consists  of  compilers  and  assemblers  for  the 
FPP  and  other  PPEs,  a  crossbar  compiler,  and  miscellaneous  debugging  tools. 

6.1.1  FPP  C  Compiler 

The  FPP  C  compiler  is  based  on  the  Portable  C  Compiler.  The  FPP  is  a  difficult  processor  to  imple¬ 
ment  C  on  since  it  is  not  capable  of  manipulating  or  addressing  8  bit  quantities.  The  FPP  supports 
only  two  data  types,  int  andfloat,  and  pointers  to  these  types.  Note  that  the  type  char  is  not  supported, 
and  therefore,  strings  are  not  supported.  The  limited  memory  size  of  the  FPP  prohibits  large  pro¬ 
grams  from  being  loaded  into  the  FPP.  Communication  routines  have  been  added  to  the  C  library 
for  the  FPP  to  support  communication  to  the  host  and  to  the  crossbar. 

6.1.2  Ada  Compiler 

An  initial  Ada  compiler  was  developed  for  the  FPP  processor.  The  output  of  the  Ada  compiler  is 
C  code,  therefore  it  is  actually  an  Ada  to  C  translator.  The  translator  mechanism  simplifies  adding 
new  processors  as  processing  nodes  to  the  PFP,  since  C  compilers  are  in  general  more  readily  avail¬ 
able  for  any  processor.  The  Ada  compiler  only  supports  a  Pascal  like  subset  of  the  Ada  language. 
Tasking  is  not  supported  by  this  compiler.  The  Ada  compiler  supports  a  one  program  per  processor 
model,  so  a  separate  program  has  to  be  written  for  each  processor,  except  where  identical  code  can 
be  repeated  on  multiple  processors.  The  internal  compiler  is  currently  being  replaced  by  a  commer¬ 
cial  Ada  to  C  compiler  to  avoid  the  development  cost  and  difficulties  associated  with  writing,  test¬ 
ing,  and  validating  an  Ada  compiler.  Communication  routines  have  been  added  to  the  Ada  library 
for  the  FPP  to  support  communication  to  the  host  and  to  the  crossbar. 

6.1.3  Macro  Assembler  and  Linker 

The  output  of  the  C  compiler  is  fed  to  an  assembler  in  order  to  produce  linkable  code.  This  assembler 
uses  a  preprocessor  to  handle  macro  expansion  then  creates  the  microcode  instructions  for  the  FPP. 
A  final  link  stage  prepares  the  code  for  loading. 

6.1.4  Crossbar  Compiler 

The  programming  model  for  a  PPE  has  four  channels.  These  are  ; 

1.  16  bit  input  from  the  host, 

2.  16  bit  output  to  the  host, 

3.  16  bit  input  from  the  crossbar,  and 

4.  16  bit  output  to  the  crossbar. 

Since  the  processor  sees  only  a  crossbar  output  channel  and  a  crossbar  input  channel,  and  no  source 
or  destination  addresses,  a  careful  job  of  coordinating  the  crossbar  data  transfers  with  the  processor 
data  is  required  to  prevent  data  from  getting  delivered  to  the  wrong  destination. 

A  compiler  is  used  to  generate  the  needed  crossbar  code.  The  crossbar  code  also  is  used  to  assign 
modules  to  physical  processors.  At  the  beginning  of  the  crossbar  code,  a  definition  line  is  used  to 
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declare  a  name  for  a  module  and  to  designate  a  physical  processor  as  the  code  destination.  This  name 
can  be  used  throughout  the  crossbar  code,  and  the  host  program.  The  definition  is  also  used  by  the 
crossbar  compiler  to  generate  a  loader  program  that  will  load  all  of  the  modules  to  the  PFP  when 
the  simulation  is  run. 

6.1.5  Tools 

Miscellaneous  software  tools  were  used  to  support  the  simulation.  Control  programs  for  the  crossbar 
sequencer  allow  the  sequencer  to  be  single  stepped,  or  stepped  slowly,  allowing  the  operator  to  ob¬ 
serve  the  status  of  the  communication  network  as  the  simulation  proceeds.  The  debugging  tools  for 
the  FPP  allow  memory  to  be  dumped  to  a  file  for  low  level  debugging.  Other  programs  examine 
the  entire  PFP  and  report  the  status  of  each  processor,  performing  sanity  checks,  processor  type 
checking,  and  processor  network  consistency  verification. 

6.2  Simulation  Code 

The  simulation  used  three  modules  to  generate  the  actual  simulation,  and  one  module  to  provide  the 
interface  to  the  host.  Crossbarcode  was  necessary  to  run  the  communication  network.  The  host  also 
had  to  be  programmed  to  load  the  appropriate  parameters  for  the  simulation,  and  display  the  simula¬ 
tion  results  as  the  program  ran.  A  block  diagram  of  the  program  model  is  shown  in  figure  5. 

6.2.1  Threat  Generation  Module 

The  threat  module  was  responsible  for  creating  the  trajectory  for  one  target.  Multiple  targets  were 
created  by  using  multiple  threat  modules  on  separate  processor  nodes.  The  target  followed  an  exo- 
atmospheric  ballistic  trajectory.  The  threat  module  computed  the  current  position  for  a  single  target. 
With  data  received  from  the  rest  of  the  simulation,  the  threat  module  then  computed  its  relative  posi¬ 
tion  to  the  interceptor.  Using  this  data,  the  projection  of  the  target  onto  the  FPA  of  the  interceptor 
was  computed.  This  projection  was  then  transferred  over  the  crossbar  to  the  threat  module. 

6.2.2  Tracking  Module 

This  module  received  the  projection  of  each  threat  onto  the  FPA.  It  ran  in  two  basic  stages.  Initially 
the  tracking  module  did  not  provide  any  course  corrections,  but  simply  built  up  track  files  of  the  tar¬ 
get  position  based  on  relative  motion  across  the  FPA.  After  a  certain  number  of  frames,  the  tracking 
algorithm  started  sending  velocity  corrections  to  the  guidance  module  to  intercept  a  selected  target. 

6.2 3  Guidance  Module 

The  guidance  module  received  the  velocity  corrections  from  the  tracking  module,  and  implemented 
these  corrections  within  the  maximum  thrust  constraint  of  the  thrusters.  The  guidance  module  main¬ 
tained  the  actual  interceptor  position,  and  transmitted  this  over  the  crossbar  to  the  threat  generation 
module(s)  to  close  the  loop  for  the  simulation. 

6.2.4  Host  Interface  Module 

The  host  interface  module  received  data  from  the  threat  generation  module  and  the  host  guidance 
module  for  possible  display  by  the  host.  Since  the  host  processor  does  not  provide  sufficient  speed 
to  display  the  data  from  every  simulation  cycle,  this  module  provided  a  throw-away  buffer,  where 
data  not  requested  by  the  host  was  overwritten  by  the  data  from  the  next  cycle  of  the  simulation. 
This  module  had  an  alternate  mode  which  forced  the  simulation  to  run  slow  enough  for  the  host  to 
receive  every  N*  frame,  where  N  was  a  parameter  that  was  downloaded  from  the  host  at  the  begin¬ 
ning  of  the  simulation.  This  module  attempted  to  format  the  data  as  much  as  possible  to  minimize 
the  computation  necessary  on  the  host  in  order  to  provide  maximum  display  update  speed. 


14 


6.2.5  Host  Programs 

The  host  program  provides  the  host  side  code  for  running  the  simulation  and  displaying  the  results. 
The  host  program  initially  calls  a  routine  created  by  the  crossbar  compiler  to  load  all  of  the  code  onto 
the  PFP’s  processors.  The  host  must  then  read  a  simulation  data  file  and  transmit  all  the  initialization 
data  to  the  appropriate  processors.  This  allows  several  parameters  of  the  simulation  to  be  changed 
without  recompiling  the  simulation  by  just  editing  this  file  and  re-running  the  simulation.  The  host 
must  then  provide  the  graphic  display  of  the  data  as  the  simulation  runs.  When  the  host  is  ready  for 
a  snapshot  of  the  simulation,  it  sends  a  request  to  the  host  interface  module  on  the  PFP  for  the  data 
from  one  simulation  cycle.  This  data  is  displayed  and  checked  for  end  of  simulation.  This  continues 
until  the  end  of  the  simulation,  or  the  operator  aborts  the  simulation. 

6.2.6  Crossbar  Code 

The  crossbar  code  is  used  to  program  the  crossbar  and  crossbar  sequencer.  This  code  is  responsible 
for  assuring  that  the  data  is  transferred  between  the  correct  modules.  Running  a  different  number 
of  threats  requires  the  crossbar  code  to  be  recompiled  with  a  new  parameter  reflecting  the  new  num¬ 
ber  of  threats.  Another  parameter  in  this  code  causes  the  simulation  to  use  the  actual  flight  processor 
harware  instead  of  running  the  track  module  on  a  PPE. 


Interceptor  position 


Figure  5.  Simplified  Program  Block  Diagram 
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7.  FLIGHT  PROCESSOR 

This  phase  of  the  simulation  was  designed  to  demonstrate  both  the  hardware-in-the-loop  capabili¬ 
ties  of  the  PFP  and  to  benchmark  the  FPP  hardware  and  software  against  a  flight  qualified  GN&C 
processor.  For  this  phase,  a  commercially  available  flight  processor  was  chosen.  A  GN&C  to  PFP 
interface  was  designed  and  constructed.  Finally,  a  simulation  scenario  was  designed  and  coded. 

7.1  Hardware 

7.1.1  GN&C  Processor 

The  Honeywell  (Sandia)  S5  was  chosen  as  the  flight  qualified  GN&C  processor.  Far  from  a  bare- 
bones  computer,  the  design  of  the  S5  is  entirely  modular.  Modular  construction  allows  the  system 
to  be  configured  based  on  deployment  objectives.  Georgia  Tech’s  S5  was  configured  with  the  fol¬ 
lowing  modules. 

1.  D  PM-H1 17X  Processor  Module 

2.  D  SI-H048X  System  I/O  Module 

3.  D  EM-H011X  Expansion  Memory  Module 

4.  DUM-H039X  Utility  Module 

5.  D  FF-H010X  Mil-Std  1553  Bus  Interface  Module 

For  the  simulation  only  the  first  two  modules  were  used.  The  processor  module  is  based  on  a  20MHz 
Motorola  68020.  Included  on  the  module  are  a  Motorola  68881  floating  point  co-processor  and 
128kB  of  ram.  The  processor  module  is  rated  at  800  KFLOPs. 

The  I/O  module  contains  a  wealth  of  resources.  Included  on  this  module  are  five  serial  ports,  a  paral¬ 
lel  port,  timers,  interrupt  inputs,  discrete  inputs  and  discrete  outputs.  The  parallel  port  was  chosen 
as  the  interface  port  between  the  S5  and  the  PFP.  The  parallel  port  is  essentially  a  slow  extension 
of  the  S5’s  internal  busses.  Through  this  port,  address,  handshake,  reset  and  data  are  all  available 
for  use  by  an  external  device.  Using  this  port  as  the  basis  for  the  interface  allowed  the  S5  to  read 
and  write  data  to  the  PFP  interface  through  a  single  memory  mapped  location. 

7.1.2  PFP  Interface 

One  of  the  attractive  features  of  the  PFP  is  the  ease  with  which  devices  can  be  added  as  parallel  pro¬ 
cessing  elements.  The  interface  hardware  is  simply  a  protocol  transformer  between  a  device  port 
and  one  of  the  PFP  sequencer/crossbar  ports.  The  PFP  sequencer/crossbar  protocol  is  defined  by 
the  protocol  of  the  Advanced  Micro  Devices  AM2950  (bi-directional  I/O  port  with  handshake).  It 
is  generally  preferable  to  use  the  AM2950  when  designing  a  PFP  interface.  A  schematic  of  the  inter¬ 
face  is  shown  in  Figure  6.  The  circuit  uses  two  AM2950s,  some  buffer/drivers,  three  connectors  and 
a  single  20  pin  programmable  logic  device  (PLD).  The  schematic  shows  the  ease  with  which  any 
parallel  device  can  be  added  as  a  PPE. 
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Figure  6.  Flight  Processor  connection  board 


7.2  Software 

Software  for  the  S5  is  broken  down  into  three  categories.  The  first  is  a  description  of  the  S5  Gateway 
control  software.  The  second  is  a  description  of  the  compiler.  The  third  is  a  description  of  the  simu¬ 
lation  program. 

7.2.1  S5  Gateway 

The  laboratory  setup  for  the  S5  included  an  IBM  PC-AT  running  the  S5  Gateway  software  under 
MS-DOS.  The  S5  Gateway  was  provided  by  Honeywell  and  controlled  all  host  interaction  with  the 
S5.  The  Gateway  software  is  used  to  load  and  run  programs.  However,  its  most  useful  feature  turned 
out  to  be  the  debugging  environment  The  debugging  environment  for  the  S5  is  powerful.  It  allows 
a  programmer  to  set  a  variety  of  different  break— points  and  single  step  through  instructions  once  a 
break-point  is  reached.  A  memory  viewer  that  formats  memory  in  a  variety  of  different  formats  is 
also  available.  Memory  can  be  viewed  as  (float),  (int),  (double),  (long  int),  etc.  The  memory  can 
also  be  ”un-compiled”  to  show  the  assembly  language  instructions  associated  with  a  memory  pat¬ 
tern.  The  contents  of  any  memory  location  can  be  changed  and  an  entire  ’’snap  shot”  of  the  memory 
can  be  saved  to  disk  for  loading  at  a  later  time.  The  debugging  tools  were  used  extensively  in  the 
development  of  the  simulation  software. 
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7.2.2  Compiler 

The  recommended  compiler  for  the  S5  is  the  Oasys/GreenHill  C  compiler  running  on  a  micro  VAX 
under  VMS.  Due  to  licensing  problems  within  the  Georgia  Tech  legal  department,  Honeywell  com¬ 
piled  the  source  code  used  for  the  simulation.  The  turnaround  time  for  this  was  approximately  one 
week.  This  made  the  rewrite-compile-run-debug  cycle  too  long  for  more  than  a  few  iterations. 
Fortunately,  the  application  code  could  be  run  and  verified  on  the  FPP  prior  to  compilation  at  Honey¬ 
well.  Even  with  this  precaution,  one  bug  in  the  S5  code  occcurred.  Since  time  constraints  made 
another  compilation  cycle  impossible,  the  debugging  tools  were  brought  to  bear  with  good  results. 
The  bug  turned  out  to  be  a  tricky  but  interesting  one.  The  problem  manifested  itself  whenever  the 
S5  sent  (float)  data  to  the  interface.  The  procedure  call  to  send  a  (float)  is  shown  below, 
float  sendjloat(x) 

float  x;  /*  cast  as  32  bit  float  7 

{ 

unsigned  short  ‘ptWord;  /*  unsigned  short  =  16  bits  so  pointer  to  a  1 6  bit  quantity  7 
ptWord  =  &x;  r  assign  a  pointer  to  x  7 

send_word(  ‘(ptWord) );  /*  send  out  first  1 6  bits  of  32  bits  7 

send_word(  *(ptWord+1) );  /*  send  out  second  16  bits  of  32  bits  7 

} 

The  data  seen  at  the  interface  using  this  code  turned  out  to  be  the  two  most  significant  words  of  a 
64  bit  IEEE  real.  As  it  turns  out,  all  reals  are  passed  in  C  as  (double).  The  procedure  is  then  supposed 
to  cast  the  variable  into  the  proper  size  based  on  the  variable  type  defined  after  the  procedure  defini¬ 
tion.  The  compiler  in  this  case  did  not  generate  the  code  to  change  x  into  a  (float).  Thus,  ptWord 
was  pointing  to  a  (double)  instead  of  a  (float).  The  C  work  around  for  this  is  easy.  The  corrected 
code  looks  like  the  following, 
float  send_float(x) 

float  x;  /*  does  not  cast  as  32  bit  float  COMPILER  ERROR  7 

{ 

float  NewX;  /*  new  (float)  variable  7 

unsigned  short  ‘ptWord;  /*  unsigned  short  =16  bits  sp  pointer  to  16  bit  quantity  7 
NewX  =  x;  /*  NewX  is  now  (float)x  7 

ptWord  =  &NewX;  /*  assign  a  pointer  to  NewX  7 

send_word(  ‘(ptWord) );  /*  send  out  first  1 6  bits  of  32  bits  7 

send_word(  *(ptWord+1) );  /*  send  out  second  16  bits  of  32  bits  7 

} 

This  C  code  has  been  compiled  and  verified. 

The  binary  work  around  is  a  little  more  involved.  The  code  could  not  be  altered  at  the  procedure 
level,  but  had  to  be  altered  at  every  call  to  the  procedure.  The  change  involved  reworking  the  stack 
so  that  the  (float)  variable  was  pushed  on  the  stack  instead  of  the  (double)  variable.  This  change  was 
possible  because  the  instructions  occupied  the  same  bit  lengths.  The  debugging  tools  and  the  com¬ 
piler  output  files  made  the  job  of  finding  and  correcting  this  bug  relatively  straight  forward. 

7.3  Programming 

The  algorithms  that  were  run  on  the  S5  were  identical  to  the  algorithms  described  Section  6.  The 
difference  is  that  the  S5  algorithms  were  coded  in  C  instead  of  Ada. 
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8.  HWIL  TEST  RESULTS 


8.1  Simulation 

Because  this  simulation  was  intended  to  be  a  demonstration  of  the  PFP  as  a  simulation  engine,  the 
simulation  avoided  using  detailed  models  which  would  have  made  the  simulation  more  realistic  but 
would  have  prevented  the  simulation  from  being  used  as  a  demonstration.  Because  of  these  limita¬ 
tions,  the  simulation  results  of  the  interceptor  in  themselves  are  not  meaningful  for  any  real  problem. 

8.2  Test  Cases:  Host  Only 

Running  the  simulation  initially  on  the  host  was  invaluable  for  porting  the  initial  algorithms  and  for 
developing  new  algorithms.  The  multi-windowed  environment  with  source  level  debuggers  al¬ 
lowed  the  code  to  be  edited,  compiled,  and  debugged  faster  than  could  be  done  on  the  PFP. 

Although  running  on  the  host  was  faster  for  the  edit,  compile  and  debug  stages,  the  host  was  soon 
found  to  be  too  slow  when  the  simulation  actually  had  to  be  run.  The  simulation  was  run  on  the  host 
with  only  one  target.  The  host  could  only  run  the  program  at  a  3  Hz  frame  rate,  much  slower  than 
the  100  Hz  real-time  simulation. 

8.3  Test  Cases:  PFP 

When  ported  to  the  PFP,  the  code  was  already  operational,  so  the  major  task  was  to  develop  the  code 
for  the  communication  network.  The  PFP  provided  the  fastest  environment  for  running  the  simula¬ 
tion.  The  PFP  simulation  used  a  timer  algorithm  to  prevent  the  simulation  from  running  faster  than 
real-time  speed  of  1 00  Hz.  The  simulation  would  run  real-time  up  to  the  maximum  possible  number 
of  threats  (26).  In  order  to  test  the  speed  of  the  simulation,  the  simulation  frame  rate  was  increased 
to  160  Hz  with  26  threats  before  real-time  performance  was  lost.  Analysis  of  the  simulation  re¬ 
vealed  that  the  bottleneck  at  this  speed  was  the  time  to  transfer  data  to  the  host. 

The  buffering  program  would  only  send  a  data  frame  to  the  host  when  the  host  requested  it,  but  once 
a  data  frame  was  requested,  it  had  to  be  completely  transmitted  before  the  simulation  could  proceed. 
The  time  to  send  one  frame  to  the  host  exceeded  the  cycle  time  when  the  simulation  was  run  at  over 
160  Hz.  At  100  Hz  the  PFP  would  probably  be  able  to  track  40  to  50  targets.  Since  the  tracking 
algorithm  is  not  linear  in  computational  requirements  with  respect  to  the  number  of  threat  objects, 
it  is  difficult  to  estimate  the  exact  cutoff. 

8.4  Test  Cases:  S-5/PFP 

With  the  simulation  running  on  the  PFP  and  the  Sandia  S-5  executing  the  tracking  algorithm,  the 
maximum  number  of  threats  that  could  be  handled  in  real  time  was  six  threats.  Attempting  to  run 
with  more  than  six  threats  dropped  the  simulation  out  of  real-time.  This  gives  a  real  performance 
measure  of  this  processor  for  use  as  a  KEW  flight  processor,  which  is  critical  in  today’s  atmosphere 
of  inflated  MFLOPS  ratings. 
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9.  APPENDIX 


9.1  Crossbar  code 

#indude  "threats. h" 

#ifndcf  threats 

#crror  "#define  threats  <numbei>"  missing  from  "threats. hM  file 
#endif 

#define  THREAT(N)  Threat##N  is  threat  on  p##N 

#if  USE_S5  =  TRUE 
[Use_S5  =  True] 

#  define  S5yl4 
#endif 


QockS  is  clocks  on  p26 

QockR  is  clockR  on  p27 

#if  USE_S5  =  TRUE 
[Use_S5  ass  True] 

Track  is  s5_control  on  p28 

#else 

[Use_S5  =  False] 

Track  is  track  onp28 

#endif 

Los_gnc  '  is  los^gnc  on  p29 

HostIO  is  hostio  onp31 

THREAT(0) 

#  define  Threats_l  ThreatO 

#  define  Threats  Threats_l 

#if  threats  >  1 
THREAT(l) 

#  define  Threats_2  Threats_l  Threatl 

#  undef  Threats 

#  define  Threats  Threats_2 
#endif 

#if  threats  >  2 
THREAT(2) 

#  define  Threats_3  Threats_2  Threat2 

#  undef  Threats 

#  define  Threats  Threats  3 
#endif 

#if  threats  >  3 

THREAT(3) 

#  define  Threats_4  Threats_3  Threat3 

#  undef  Threats 

#  define  Threats  Threats_4 

#endif 

#if  threats  >  4 
THREAT(4) 

#  define  Threats_5  Threats_4  Threat4 

#  undef  Threats 

#  define  Threats  Threats  5 

#endif 

#if  threats  >  5 

THREAT(5) 

#  define  Thrcats_6  Threats_5  Threats 

#  undef  Threats 

#  define  Threats  Threats  6 

#endif 

#if  threats  >  6 

THREAT(6) 

#  define  Thrcats_7  Threats_6  Threat6 

#  undef  Threats 

#  define  Threats  Threats_7 

#endif 
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#if  threats  >  7 
THREAT(7) 

#  define  Threats  _8  Threats_7  Threat7 

#  undcf  Threats 

#  define  Threats  Threats_8 
#endif 

#if  threats  >  8 
THREAT(8) 

#  define  Threats_9  Threats_8  Threat8 

#  undef  Threats 

#  define  Threats  Threats_9 

#endif 

#if  threats  >  9 
THREAT(9) 

#  define  Threats_10  Threats_9  Threat9 

#  undef  Threats 

#  define  Threats  Threats_10 
#endif 

#if  threats  >  10 
THREAT(IO) 

#  define  Threats_ll  Threats_10  Threat  10 

#  undef  Threats 

#  define  Threats  Threats_10 
#endif 

#if  threats  >  11 
THREAT(ll) 

#  define  Threats__12  Threats_l  1  Threat  1 1 

#  undef  Threats 

#  define  Threats  Threats_12 
#endif  . 

#if  threats  >  12 
THREAT(12) 

#  define  Threats_13  Threats_12  Threat  12 

#  undef  Threats 

#  define  Threats  Threats_13 
#endif 

#if  threats  >  13 

THREAT(13) 

#  define  Threats_14  Threats_13  Threat  13 

#  undef  Threats 

#  define  Threats  Threats_14 
#endif 

#if  threats  >  14 
THREAT(14) 

#  define  Threats_l5  Threats_14  Threat  14 

#  undef  Threats 

#  define  Threats  Threats_15 
#endif 

#if  threats  >  15 
THREAT(15) 

#  define  Threats_16  Threats_15  Threat  15 

#  undef  Threats 

#  define  Threats  Threats_16 
#endif 

#if  threats  >  16 
THREAT(16) 

#  define  Threats_17  Threats_16  Threatl6 

#  undef  Threats 

#  define  Threats  Threats_17 
#endif 

#if  threats  >  17 
THREAT(17) 

#  define  Threats_18  Threats_17  Threat  17 

#  undef  Threats 

#  define  Threats  Threats_18 
#endif 

#if  threats  >  18 
THREAT(18) 

#  define  Threats_19  Threats^  18  Threat  18 

#  undef  Threats 

#  define  Threats  Threats_19 
#endif 

#if  threats  >  19 


THREAT(19) 

#  define  Threats_20  Threats_19  Threat  19 

#  undef  Threats 

#  define  Threats  Threats_20 

#endif 

#if  threats  >20 
THREAT(20) 

#  define  Threats_21  Threats_20  Threat20 

#  undef  Threats 

#  define  Threats  Threats_21 

#endif 

#if  threats  >21 
THREAT(21) 

#  define  Threats_22  Threat s_21  Threat21 

#  undef  Threats 

#  define  Threats  Threats  22 

#endif 

#if  threats  >22 
THREAT(22) 

#  define  Thrcats_23  Threats_22  Threat22 

#  undef  Threats 

#  define  Threats  Threats_23 

#endif 

#if  threats  >23 
THREAT(23) 

#  define  Threats_24  Threats_23  Threat23 

#  undef  Threats 

#  define  Threats  Threats  24 

#endif 

#if  threats  >  24 

THREAT(24) 

#  define  Threats_25  Threats_24  Threat24 

#  undef  Threats 

#  define  Threats  Threats  25 

#endif 

#if  threats  >25 
THREAT(25) 

#  define  Thrcats_26  Threats_25  Threat25 

#  undef  Threats 

#  define  Threats  Threats  26 

#endif 

#if  threau  >26 

#enor  P26  is  currently  used  by  an  Clock  Sender! 
#endif 


#if  USE_S5  —  TRUE 

[Use_S5  =  True] 

#define  delta_phi  \ 
cyde  Los  _gnc.2 
cyde  Track,  2 

#define  ddu.theta  \ 
cyde  Los _gnc2 
cyde  Track.2 

#define  pixel  x(N)  \ 
cyde  Thrcat##N 
cyde  Track 
cyde  Track 

#define  pixel_y(N)  \ 
cyde  Threat ##N 
cyde  Track,  2 

#define  S5_iIcoorx  \ 
cyde  Track.2 

#define  S5_iIcooiy  \ 
cyde  Track.2 

#deftne  cArea(N)  \ 
cyde  Threat##N 
cyde  Track 


— >  Track  Threats;  [delta_phi  as  float]  \ 
— >  S5;  [delta_phi  as  float] 


— >  Track  Threats;  [delta_theta  as  float]  \ 
— >  S5;  [delu_theu  as  float] 


— >  Track  HostIO;  [pixel_x  as  short]  \ 
— >  S5;  [iAcoorx  as  float]  \ 

— >  S5;  [iAcoorx  as  float] 


— >  Track  HostIO;  [pixel_y  as  short]  \ 


— : >  S5; 

[iAcoory  as  float] 

— >  S5; 

[ilcoorx  as  float] 

— >  S5; 

[ilcoory  as  float] 

— >  Track  HostIO;  [cArea  as  short]  \ 
— ^>  S5;  [cArea  as  short] 
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#define  cIntensity(N)  \ 

cyde  Tlireat##N.2  — >  Track  HostIO;  [Intensity  as  int]  \ 

cyde  Track  — >  S5;  [Intensity  as  short] 

#define  cRange(N)  \ 

cyde  Threat##N.2  — >  Track  HostIO;  [cRange  as  float]  \ 

cyde  Trfick  — >  S5;  [cRange  as  short] 

#define  x_dot\ 

cyde  S5.2  — >  Track;  [x_dot  as  float]  \ 

cyde  Track.2  — >  Los_gnc;  [x_dot  as  float] 

#definey_dot\ 

cyde  S5.2  — >  Track;  [y_dot  as  float]  \ 

cyde  Track.2  — >  Los_gnc;  [y_dot  as  float] 

#define  phi_dot\ 

cydeS5.2  — : >  Track;  [phi_dot  as  float]  \ 

cyde  Track.2  — >  Los_gnc;  [phi__dot  as  float] 

#define  theta_dot\ 

cyde  S5.2  — >  Track;  [theta_dot  as  float]  \ 

cyde  Track.2  — >  Los _gnc;  [theta_dot  as  float] 

#define  init_s5  \ 


cyde  Track.2 

— : >  S5; 

[threat  intensity]  \ 

cyde  Track.2 

— >  S5; 

[dt]  \ 

cyde  Track.2 

— >  S5; 

[TargetSelectionTinie  ]  \ 

cyde  Track 

— >  S5; 

[Num_obj  ]  \ 

cyde  Track.2 

— >  S5; 

[FinalAcquisitionRange  ]  \ 

cyde  Track 

— >  S5; 

[TrackingRange  ] 

#define  Threat(N)  \ 
pixd_x(N)\ 
pixd_y(N)\ 

S5_iIcoorx\ 

S5_iIcoory\ 
cArea(N)  \ 
cIntensity(N)  \ 
cRange(N)\ 
threat_coor(N)[0]  \ 
threat_coor(N)[  1  ]  \ 
threat_coor(N)[2]  \ 
miss_distance(N) 

[End  of  if  Use_S5  =  True] 

#dse  [if  Use_S5  =  True] 

[Use_S5  =  False] 

#define  delta _phi  \ 

cyde  Los_gnc.2  — >  Track  Threats;  [delta_phi  as  float] 

#define  delta  jtheta  \ 

cyde  Los_gnc.2  — >  Track  Threats;  [delta_theta  as  float] 

#define  pixel_x(N)  \ 

cyde  Threat##N  — >  Track  HostIO;  [pixel_x  as  short] 

#define  pixel_y(N)  \ 

cyde  Threat##N  — >  Track  HostIO;  [pixel_y  as  short] 

#define  cArea(N)  \ 

cyde  Threat##N  — >  Track  HostIO;  [cArea  as  short] 

#define  dntensity(N)  \ 

cyde  Threat##N.2  — >  Track  HostIO;  [Intensity  as  int] 

#define  cRange(N)  \ 

cyde  Threat##N.2  — >  Track  HostIO;  [cRange  as  float] 

#define  x_dot  \ 

cyde  Track.2  — >  Los _gnc;  [x_dot  as  float] 
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#define  y_dot  \ 

cydc  Track.2  — >  Los _gnc;  [y_dot  as  float] 


#define  phi_dot  \ 
cyde  Track.  2 

— >  Los _gnc; 

[phi  jJot  as  float] 

#define  theUjdot  \ 
cyde  Track.2 

— >  Los _gnc; 

[theU_dot  as  float] 

# define  Thrcat(N)  \ 

pixel  x(N)\ 
pixel_y(N)\ 
cArea(N)  \ 
dntensity(N)  \ 
cRange(N)\ 
thieat_coor<N)[0]  \ 
thrcat_coor(N)[lj  \ 
threat_coor(N)[2]  \ 
missjdistance(N) 

(end  of  if  UseJ55  —  True  else  clause] 
#endif  [if  UsejSS  =  Tnie] 

#define  delta_x  \ 

cyde  Los _gnc.2 

— >  Threats; 

[delta_x  as  float] 

#define  delu_y  \ 

cyde  Los_gnc.2 

— >  Threats; 

[delta_y  as  float] 

# define  missle_coor  \ 
cyde  Thieat0.2 

— >  HostIO; 

[float] 

#define  threat  cooi(N)  \ 
cyde  Threat##N.2 

— >  HostIO; 

[float] 

#define  miss_distance(N)  \ 

cyde  Thrcat##N.2  — >  HostIO; 

[float] 

#define  lockstepX 
cyde  QockS 

— >  QockR; 

[dummy  short] 

#define  Basic_Sends  \ 
della jhetaX 
delta__phi  \ 
delta_x  \ 
delta_y 


[  ****************************************************************************  ] 

#if  USE_S5  =  TRUE 
[Usc_S5  =  True] 

(Initialize  s5] 
init  s5 
#endif 


[lock  the  step] 
lockstep 
[basic_sends] 

Basic_Sends 

[Threat  0] 

pixel_x(0) 

pixel_y(0) 

#  if  USE  S5  =  TRUE 

[Use_S5  =  True] 
S5JIcoorx 
S5  ilcoory 

#  endif 

cArea(0) 

clntensity(O) 
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cRange(0) 

t 

Missile  Coordinate [n]  is  sent  by  ThreatO  only 
Cycles  shared  with  TrackObj  sending  to  Los__gnc 

] 

missle_coor[0] 

missle_coor[l] 


missle_coor[2] 

threat_coor(0)[0] 

threat_coor(0)[l] 

threat_coor(0)[2] 

miss_distance(0) 

#if  threats  >  1 
Threat(l) 

#endif 

#if  threats  >  2 
Threat(2) 

#endif 

#if  threats  >  3 
Threat(3) 

#endif 

#if  threats  >  4 
Thrcat(4) 

#endif 

#if  threats  >  5 
Threat(5) 

#endif 

#if  threats  >  6 
Threat(6) 

#endif 

#if  threats  >  7 
Threat(7) 

#endif 

#if  threats  >  8 
Threat(8) 

#endif 

#if  threats  >9 
Threat(9) 

#endif 

#if  threats  >  10 
Threat(lO) 

#endif 

#if  threats  >11 
Threat(ll) 

#endif 

#if  threats  >  12 
Threat(12) 

#endif 

#if  threats  >  13 
Threat(13) 

#endif 

#if  threats  >  14 
Threat(14) 

#endif 

#if  threats  >  15 
Threat(15) 

#endif 

#if  threats  >  16 
Threat(16) 

#endif 

#if  threats  >  17 
Threat(17) 

#endif 

#if  threats  >  18 
Threat(l  8) 

#endif 

#if  threats  >  19 
Threat(19) 


#endif 

#if  threats  >  20 
Threat  (20) 

#endif  . 

#if  threats  >  21 
Threat  (21) 

#endif 

#if  threats  ^  22 
Threat(22) 

#endif 

#if  threats  >23 
Threat  (23) 

#endif 

#if  threats  >  24 
Thrcat(24) 

#endif 

#if  threats  >  25 
Threat(25) 

#endif 

#if  threau  >26 
Threat(26) 

#endif 

#if  threats  >27 
Threat  (27) 

#endif 

#if  threats  >  28 
Threat  (28) 

#endif 

x_dot 

y_dot 

phijdot 

theUjdot 

9.2  C  Code 

9.2.1  Main  Host  Program  closedloop.c 

#in elude  "trackJT 


Take  earth’s  center  as  x,y, ^(0,0,0)  for  earth  relative  cordinates 
fpa  coordinates  is  a  cartesian  coordinate  system  with 
the  orgin  on  the  interceptor.  The  z  axis  of  this  coordinate 
system  extends  through  the  center  of  the  fpa  out  into 
the  field  of  view,  normal  to  the  fpa. 

The  x  axis  is  runs  through  the  extended  plane  of  the  fpa  horizontally, 
and  the  y  axis  vertically.  In  order  for  this  to  be  a  right  handed 
cordinate  system,  x  is  positive  toward  the  right,  but  y  is  positive 
downward. 

V 


extern  void  Initialize_track_object(); 
extern  void  track_object(); 
extern  float  host_io(); 
extern  struct  hostioType  hostio; 
extern  struct  ControlType  Control; 

FILE*fp;  /*  input  file  */ 

float  time_step;  /*  Time  increment  per  simulation  step  */ 

struct  CentroidDataiype  Centroid  Data  [100]; 
struct  initializeType  initialize; 
struct  ControTfype  Control; 
int  CentroidDataPtrJwstioPtr, 

int  run  Jength,  ent; 

main(argc,argv) 
int  arge; 
char  **argv; 

{ 

float  t,  prev_miss_distancermiss; 
char  line[  100]; 
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if(argol)  initialize_main_loop(argv[l]); 

else  initialize_mam_loopC’infilc”); 

initialize_threat_object(); 
inilializc_track_objectO; 
initialize JiostioO; 
initializejos _gncO; 

prcv_miss_di  stance  =  2e30; 
miss  =  le30; 

while  (hostio.miss_distance<0)  { 

CentroidDataPtr  =  O,  /*  Reset  Centroid  Data  pointer  */ 
hostioPtr  =  0; 

threat_objectO;  I*  Generate  a  Threat  Centroid  */ 
CentroidDataPtr  =  0 ;  /*  Reset  Centroid  Data  pointer  */ 
hostioPtr  =  0; 

track_objectO;  /*  Trade  object  &  Select  Target  *1 

los_gnc0; 

t  +=  time_step;  f*  Move  time  forward  */ 

hostjoQ; 


} 

gets  (line);  f*  Hold  until  CR  pressed  on  Keyboard  */ 


iniUalize_main_lo°p  (infile) 

char  *  infile; 

{ 

FILE  *ifp; 
int  a; 

if  (NULL  =  (ifp=fopen(infile”n))  { 

fprintf(stderr, "Could  not  open  input  file  %sW, infile); 
cxit(l); 

} 

a  =  fscanf(ifp”  Threat  Coordinates:  %f  %f  %f\ 

&(initialize.threat_coor[03)  , 

«fe(initialize.threat_coor[  1  ])  t 
&(initialize.threat_coor[2])  ); 
a  +=  fscanf(ifp,"  Threat  Velocity:  %f  %f  %f\ 
&(initialize.threat_velocity[0]) , 

&(initialize.threat_velocity[l]) , 

&(initialize.threat_velocity[2]) ); 
a  +=  fscanf(if^),”  Threat  Size,  Intensity:  %f  %f\ 

&(initialize.threat_size) , 

&(initialize.threat_in tensity)  ); 

a  +=  fscanf(ifp,”  Missile  Orientation:  %f  %f  %f  %f  %i  %f  %f  %f  %f\ 
&(initialize.missile_aim[0]  [0]), 

&(initialize.missile_aim[0][l]), 

&(initialize.missile_aim[0]  [2]), 

&(initialize.missile_aim[  1  ]  [0] ), 

&(initialize.missile_aim  [  1  ]  [  1  ]), 

&(initialize.missile_aim  [  1  ]  [2]), 

&(initialize.missile_aim[21[0]), 

&(initialize.missile_aim[2][  1  ]), 

&(initialize.missile_aim[2][2])  ); 
a  +=  fscanf(ifp,”  Missile  Coordinates:  %f  %f  %f  \ 
&(initialize.missile_coor[Oj)  , 

&(initialize.missile_coor[  1])  , 

&(initialize.missile_coor[2])  ); 
a  +=  fscanf(i^)”  Missile  Velocity:  %f  %f  %f\ 
&(initialize.missile_velocity[0]), 

&(initialize.missile_velodty  [  1  ] ), 

&(initialize.missile_velodty  [2] )  ); 
a  +=  fscanf(if^1”  Miss  Distance:  %f  \&(initialize.MissDistance)); 
a  +=  fscanf(ifp  ”  Time  Step:  %f  \&time_step); 

a  +=  fscanf(ifp”  Observation  Orientation:  %f  %f  %f  %f  %f  %f  %f  %f  %V\ 
&(initialize.panview_prient[0]  [0]), 

&(initialize.panvie  w_orient[0j  [  1  ]), 

&(initialize.panview_orient  [0]  [2] ), 

&(initialize.panview_orient[  1  ]  [0]), 
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&0niliali2C.panview_oricnt  [  1  ]  [  1  ])f 
&(mitialize.panview_orient  [  1  j  [2] ), 
&(initiaIize.panview_orient  [2]  [0]), 
A0nitiilizc.panview_oricnt[2]  [1  j), 
&(initia]ize.panview_orienl[2)  [2]) ); 
a  +=  fscanf(ify Observation  Point:  %f  %(  %f  \ 

.  &(mitialize.observation_jx>int[0]) , 
&(iiutialize.observationjpoint[  1  ] ) , 
&(initialize.observation_point(2]) ); 
a  +=  fscanf(ifptM  Panview  FOV:  %{  %f\ 
&(initialize.panview_view), 
&(initialize.panview_range) ); 
a  +=  fscanfQfp"  Angular,  Linear  Nav  Constants:  %f  %V\ 
&(imiializejuig_nav_const)1 
&(initialize.lin_ntv_const)); 
a  +=  fscanf(if^,"  Thruster_interval:  %f", 
&^nitialize.thnjster_interval)); 
a  +=  fscanffi^),**  Display  Interval:  %P\ 

&(friitialize.display_inteival)  ); 
a  +=  fscanf(if^>”  Target  Selection  Time:  %P, 
&(mitialize.target_»elect_time) ); 
a  +=  f*canfO^>” Maximum  Divert:  %f\ 

AQnitialize  jnax^diveit) ); 
a  +=  fscanf(ifp”  Number  of  Objects:  %d’\ 
A(initializeJ^um_obj)  ); 
a  +=  fscanf(if^,M  Final  Acquisition  Range:  %f\ 
A(initialize.FinalAcquisitionRange) ); 
a  +=  fscanf(if^,M  Tracking  Range:  %f\ 

A  (initialize. Tracking  Range)  ); 


initialize.  time_step  =  time_step; 
if  (  a  1=  48  )  { 

fprintf(stderr,”  Error  Parsing  Input  VP); 
exit(l); 

} 

J 


9.2.2  Host  FPA  window  Program  fpa.c 

#indude  <suntool/sunview.h> 
tindude  <suntool/canvas.h> 

#indude  <jtdioJi> 

#indude  "track  Ji” 

static  Nodfy_valuc  my_destioyerO; 
extent  Nodfy_error  notify _dispatchO; 

struct  FpaDauType  FpaData[32); 

static  Frame  frame; 
static  Canvas  canvas; 
static  Pixwin  *pw; 

char  line(  1000],coramand,name[CMS_NAMESIZE]; 

int  Jtpt_put,y_put,color, 

static  unsigned  char  red[64]  =  { 

0,255. 255, 255, 255, 255, 255, 255, 255, 255, 

255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 

255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 

255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 

255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 

255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 
255,255,255,255 


}. 

grcen[64]=  { 

0,255, 0, 8. 16, 24, 32, 40. 48, 56, 64, 72, 80, 

88, 96. 104, 112, 120, 128, 136, 144, 152, 160, 
168, 176, 184, 192, 200, 208, 216, 224, 232, 240, 

248, 255, 255, 255, 255, 255, 255, 255, 255, 255, 
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255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 

255. 255. 255. 255. 255. 255. 255. 255. 255. 255. 255 

}. 

blue[64]={ 

0,255,0, 0, 0, 0, 0, 0, 0, 0. 0, 0, 0, 0, 0, 

0, 0. 0, 0, 0, 0, 0, 0, 0, 0, 0, 0. 0, 0, 0, 

0, 0,  8, 16, 24, 32, 40, 48. 56, 64, 72,  80, 

88, 96, 104, 112, 120, 128, 136, 144, 152, 160, 

168. 176. 184. 192. 200. 208. 216. 224. 232. 240. 248. 255 


struct  prjpos  plist[4]; 

fpajnainloopO 

{ 

int  FpaPtr  =  O.type,  radius; 
register  int  ij; 

notify_dispatchO; 

pw_batch_on(pw); 

pw_writebackground(pw, 1 ,1 ,255,255 JPIX_SRC); 
while(  FpaData[FpaPtr]  .intensity  !=  0  )  { 

x_pix  =  FpaData[FpaPtr].fd_pixel_x; 
y_pix  =  FpaData[FpaPtr].fd_pixel_y; 
radius  =  FpaData[FpaPir].  radius; 
odor  =  FjpaData  [FpaPtr]  .intensity; 

.  x_pix  +=  1; 
y_pix  +=  1; 
x_pix  *=  2; 
y_pix  *=  2; 
if(radius>=l)  { 

for(i=(  -radius  +  x_pix);i<=(radius+x_pix);i++) 

pw_vector(pw,i,radius+y_pix,i,-radius+y_pix,PIX_SRC,color); 

}  else  if(  radius  <  0  )  { 

pw_vector(pw,x_pix+4,y_pix,x_pix-4,y_pix,PIX_SRC,63); 

pw_vector(pw^_pix>y_pix+4^_pix,y_pix-4,PIX_SRC,63); 

}  else 

pw_polypoint(pw^t_pix,yjpix,4,plisttPIX_SRCtPIX_COLOR  (color)); 

FpaPtr++; 

} 

p  w_baich_o£f (p  w); 
notify  _dispatch  0; 


} 

/*  An  attempt  at  peaceful  exits  */ 
static  Notify_yalue 
my_destroyer(frame,  status) 

Frame  frame; 

Destroy _stat us  status; 

{ 

if  (sums  !=  DESTROY_CHECKING)  { 
notify_stop0; 

} 

remm(notify  next_destroy_func(frame, status)); 

} 


fpa  initializeO 

{ 


I*  Initialize  plist  */ 

plist[0].x  =  0; 
plist  [Oj.y  =  0; 
plist[l].x  =  1; 
plist[l].y  =  0; 
plist[2].x  =  0; 
plist[2].y  =  1; 
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plist[3].x  =  1; 
plist[3].y  =  1; 


/*  Create  frame  and  canvas  *f 
frame  =  window_create(NULL,  FRAME, 
WINJHEIGHT,  257+20+-28, 

WINWIDTH,  257+20f  12, 

WIN JC,  FPA_WIN_Xt 

WIN_Y,  FPA  WIN  Y, 

FRAME  LABEL,  "FPA  View”, 
FRAME_NO_CONFIRM,  TRUE, 

0); 

canvas  =  window_create(frame,  CANVAS, 
CANVAS_RETA3NED,  TRUE, 
CANVAS.HEIGHT,  258, 

CANVAS_WIDTH,  258, 
CANVAS_AUTO_SHRINK,  FALSE, 
CANVAS^AUTO  EXPAND,  FALSE, 
CANVAS.MARGIN,  10, 

0); 

f*  get  the  canvas  pixwin  to  draw  into  *1 
pw  =  canvu_pixwm(canvai); 

I*  Handle  exits  peacefully  */ 

notify jnterposejdestroyjunc(frame,my_destroyer); 

/*  set  up  coLormap  */ 

strcpy(name,”fpa"); 

pw_setcmsname(pw,name); 


p  w_putcolormap(p  w,0,64,rcd,green  .blue); 

pw_vector(pw,0,0,257,0,PlX_SRC,  1 ); 
pw_vector(pw,257,0.257,257,PDC  SRC.l); 
pw_vector(pw^57,257,0^57J>IX_SRC(l); 
pw_vector(pw,0,257,0,0,PIX  SRC,  1 ); 
window_set(frame,WIN_SH6w,TRUE,0); 

} 


9.2.3  Host  Panoramic  View  pan.c 

#in elude  "trackJT 
#include  <simtool/sunviewJi> 

#include  <sun too  1/can vasJi> 

static  Nodfy_value  myjdestroyerO; 
extern  Notify  _error  notify  jdispatchO; 

struct  PanDataType  PanData[32]; 

static  Frame  frame; 

static  Canvas  canvas; 

static  Pixwin  *pw; 
char  line[1000], 

command, 

name[CMS_NAMESIZE]; 

int  x, 

xj)ix, 
y_pix, color; 
unsigned  char  ied[4]  =  { 

0,255,0,255 

}. 

green[4]=  { 

0,255,255,0 

}. 

blue[4]={ 

0,255,0,0 

}; 

pan_inidalize0 

{ 

/*  Create  frame  and  canvas  */ 
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inti; 

for(i=0;i<=32;i++)  PanData[i].miss_disunce  =  -1.0; 

frame  =  window_create(NULL,  FRAME, 

WIN  WIDTH,  1024+28, 

WIN  HEIGHT,  512+12, 

WINJC,  PANJWINJC, 

WINY,  PAN  WIN  Y, 

FRAME.LABEL,  ,fWorld  View”, 

FRAME  NO.CONFIRM,  TRUE, 

0); 

canvas  =  window_create(frame,  CANVAS, 

CANVAS  RETAINED,  TRUE, 

CANVAS  HEIGHT,  512, 

CANVAS  JVIDTH,  1024, 

CANVAS  JVUTCLSHRINK,  FALSE, 
CANVAS  AUTO  EXPAND,  FALSE, 

0); 

I*  get  the  canvas  pixwin  to  draw  into  */ 
pw  =  canva  s_pix  win(can  vas); 

/*  Handle  exits  peacefully  */ 

notify Jnterpose_destroy_func(frame^ny_destroyer); 

f*  set  up  colormap  */ 
strcpy(name  "pan”); 
pw_setamsname(pw,name); 
pw_j3Utcolomiap(pw,0,4,red, green, blue); 
window_set(frame,WIN_SHOW,TRUE,0); 


pan_mainloop0 

{ 

ini  panptr  =  0; 
float  miss_dist; 
char  frm[30]; 

notify  _dispatch0; 

pw_hatch_on(pw); 
while(  PanData[panptr].type  !=  0  )  { 
pw_put(  pw, 

PanDatafpanptr]  .pd_pixel_x, 

Pan  Data  [panptr]  .pd_pixel_y, 

PanData  [panptr]  -  type); 
miss_dist  =  PanDala  [panptr]  .miss_distance; 
if(miss_dist>0)  { 

sprintf(frm,”Miss  Distance:  %.2f  ”,miss_dist); 
pw  text(pw,20,46,PIX_SRCIPIX_COLOR(l),0,frm); 

} 

panptr++; 

} 

sprintf(fim,"Frame:  %d  ”,PanDaU[0]  .frame); 
pw-text(pw^0,18,PIX_SRCIPK_COLOR(l),0^nn); 
sprintf(fim, "Range  (m):  %8.2e  ",PanData[0].pd_range); 
pw_text(pw,20,32,PIX_SRClPIX_COLOR(l),0,frm); 

miss_dist  =  PanData[panptr].miss_distance; 
if(miss_dist>0)  { 

sprintf (frm,"Mis s  Distance:  %.2f  ”miss_dist); 
pw_text(pw,20,46,PIX_SRC  ,0/rm); 

} 

pw_batch_off(pw); 
notify  _dispatch  Q; 


} 

f*  An  attempt  at  peaceful  exits  */ 
static  Notify_value 
my_destroyer(frame,  sums) 
Frame  frame; 

Destroy_status  sums; 
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{ 


if  (status  !=  DESTROY_CHHCKING)  { 
notify_stopO; 

1 

rctum(nodfy_nexi_dcstroy_func(framc, status)); 


9.2.4  PFP  to  HOST  interface  hostio.c 

ffindude  "irackJT 

/*  This  process  is  the  host  buffer  and  conversion  routine  which 
prepares  things  for  display  on  the  ’hosts’  display  windows  */ 

static  FILE  ♦fpa,*pan; 

struct  hostioiype  hostio[32]; 

static  float  observation^]  J6vfpv_orient[3][3]; 

extern  struct  initializeiype  initialize; 

extern  struct  FpaDauTVpe  FpaData[32); 
extern  struct  PanDauType  PanData[32]; 

float  prev.distance, 
hio_xm[3]t 
hio_xo[3]t 
hiojunr[3], 
hio_xor(3]; 

int  radius, 

pixel_y. 
total Jnt, 

frame_display  _int, 
framejcnt, 

L 

pvJCEW_x, 
pv_JCEW_y, 
pv_thrcat_x, 
pv_threat_y; 
static  float  Num_obj; 

float  host  ioO 

( 

intij; 

int  fpaDptr=0,PanDptr=0; 
float  distance; 

for  (j=0, j<Num_obj  *j++)  { 

pixel_x  =  hostio(j].pixel_x; 
pixel^y  s  hostio(jl.pixel_y; 
radius  =  hostio{j]. radius; 
total_int  =  hostio(j}.totaI_intensity; 

f*  Output  the  pertinent  information  to  the  fpa  view  */ 
if(pixel_x!=-l  ){ 

FpaDau[fpaDptr].fd  _pixel_x  =  pixel_x; 

FpaDato[fpaDptr]id_pixel_y  =  128~pixel_y; 

FpaDaujfpaDptrj.  radius  =  radius; 

FpaData[fpaDptr].intensity  =  total_int; 
fpaDptr++; 

} 

FpaData[fpaDptr]  .intensity  =  0; 

for(i=0u<=2;i++)  { 

hio_xo[i)  =hostio|J].ihreat_coord[i]; 
hio_xo[i]  =  hio_xo[i]  -  observation  [i]; 

hio_solve(pv_orienUuo_xorfhio_xo); 

pv_threat_x  =  (imX  511.5  +  1024  *atan((double)(hio_xorI03/(-hio_xor[2]))) 
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/fov); 

pv  threat_y  =  (int)(  255.5  +  1024  * atan((double)(hio_xor[  1  ]/(-hio_xor{2] ))) 
/fov); 

f*  Output  the  pertinent  information  to  the  pan  view  for  the  threat  */ 
if(  (pv_threat_x  >=  0 )  &&  (pv_threat_x  <  1024 )  && 

(pv_threat_y  >=0  )  &&  (pv_threat_y  <512)  && 

(  hio_xor[2]<=0)  )  { 

PanData  [PanDptr]  .pd_pixel_x  =  pv_threat_x; 

PanData  [PanDptr]  .pd_pixel_y  =  5 12  -  pv_threat_y; 
PanData[PanDptr].type  =  2; 

PanData[PanDptr].pd_range  =  hostio[j]. range; 

PanData  [PanDptr]. miss  ^distance  =  hostio[j].miss_distance; 
PanDptr++; 

} 

} 

for(i=0;i<=2;i++)  { 

hio_xm[i]  =  hostio[0].missile_coord[i]; 
hio_xm[i]  =  hio_xm[i]  -  observation  [i]; 


hio_solve(pv_orientJiio__xmrJhio__xm); 

pv_KEW_x  =  (int)(  511.5  +  1024  *atan((double)(hio_xmr[0]/(-hio_xmr[2]))) 
/fov); 

pv  KEW_y  =  (int)(  255.5  +  1024  *atan((double)(hio_xmr[  1  ]/(-hio_xmr[2]))) 
/fov); 

I*  Output  the  pertinent  information  to  the  pan  view  for  the  KEW  */ 
if((pvJCEW  x  >=  0  )  &&  (pv_KEW_x  <  1024  )  && 

(pv_KEW_y  >=0  )  &&  (pvJCEW_y  <  512  )  && 

(  hio_xmr[2]<=0)  )  { 

PanData  [PanDptr]  .pd_pixel_x  =  pv_KEW_x; 

PanData  [PanDptr]  .pd_pixel_y  =  512  -  pv_KEW_y; 

PanData  [PanDptr]  .type  =  1; 

PanDptr++; 


PanData[0].frame++; 
PanData  [Pan  Dptr].type  =  0; 

panjmainloopO; 


fpajmainloopO; 


} 

initialize_hostioO 

{ 

intij; 

fpaJnitializeO; 

pan_initialize(); 

fov  =  tan(  (double)  ( initialize.panview_view  /  initialize.panview_range)  ); 
for(i=0;i<=2;i++)  { 

observation  [i]  =  initialize.observation_point[i]; 
for(j=0y<=2;j++) 

pv_orient[i][j3  =  initiaUze.panview_orient[i][j]; 

} 

Numjobj  =  initialize  .Num_obj; 


} 


l*  User  Cramers  rule  to  solve  ,  assumes  del  of  matrix  =  1*/ 

hio_solve(Atx,b) 

float  A[3][3],b[3],x[3]; 
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{ 


x[0]  =  ( 

b[0]  *  (  A[1][1]*A[2][2]  -  A[2][1]*A[1][2] ) 
-A[0][l]*(b{l]  *A[2][2]  - A[l][2]*b[2]  ) 
+  A[0][2]  *  (b[l]  *A[2][1] -  A[l][l]*b[21  ) 
): 


*[!]  =  ( 

A[0][0J*(b{l]  *A[2][2J  - b[2]  *A[1][2]) 

-  b[0]  *  (  A[1)[0]*A[2][2]  -  A[1][2]*A[2](0]  ) 
+  A[0][2]  *  (  A[1][0J*  b[2]-b[l]  *A[2][0]) 
); 


*[2]  =  ( 

A[0][0]  *  (  A[l][l]*b(2]  -  A[2](l]*b[l] ) 

-  A[0][1]  *  (  A[l][0]*b[2]  -bill  *A[2][0]  ) 

+  b[0]  *  (  A[1][0]*A[2)[1J  -  A[1]{1]*A[2][0] ) 

); 


} 

9.2.5  Target  Generation  Program  threatx 

#in  elude  "trackb" 

#includc  <math.h> 

#indudc  <jtdio  Ji> 

/* 

Take  earth’s  center  as  x,y,z=(0,0,0)  for  earth  relative  cordinates 

fpa  coordinates  is  a  cartesian  coordinate  system  with 
the  orgin  on  the  interceptor.  The  z  axis  of  this  coordinate 
system  extends  through  the  center  of  the  fpa  out  into 
the  field  of  view,  normal  to  the  fpa. 

The  x  axis  is  runs  through  the  extended  plane  of  the  fpa  horizontally, 
and  the  y  axis  vertically.  In  order  for  this  to  be  a  right  handed 
cordinate  system,  x  is  positive  toward  the  right,  but  y  is  positive 
downward. 


extern  void  track_object(); 

extern  struct  Centroid  Da  taType  CentroidDataflOO]; 

extern  struct  ContiolType  Control; 

extern  int  CentroidDauPtrhostioPtr, 

extern  struct  initializeType  initialize; 

extern  struct  hostioiype  hostioI32]; 

static  float  aim[3][3],  /*  orientation  of  interceptor  in  radians  */ 

xm[3],  /*  X  of  KEW  missile  World  coord  */ 

vm[3j,  /*  Vel  of  KEW  missile  World  coord  */ 

mr(3],  /*  location  of  this  object  in  missile  ref  frame*/ 

distance, 

prcvious_distance, 

intense,  /*  object  brightness  */ 

xo[3],  f*  X  of  target  */ 

vo{3],  /*  velocity  of  target  */ 

size,  I*  diameter  of  the  target  */ 

time_step,  J*  Time  step  to  lake  */ 

velocity, 

t;  /*  time  */ 


static  FILE  *infp,*outfp; 


static  int  pixel_x, 

pixcl_y. 
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radius, 

total_intensity; 
float  computc_miss_distance(); 

threat_objectO 

static  float  thetax,thetay;  f*  interceptor  rotation  in  radians  *1 
static  ini  ij; 

thetax  =  Control.delta_theta; 
thetay  =  Control  delta  jphi; 

fprintf(infp  "thetax  =  %f\n",thetax); 
fprintf(infp,"thetay  =  %f\n",thetay); 
fflush(infp); 

t  +=  time_step; 

baUistic(xo,vo,t);  f*  computer  ballistic  path  of  this  object  *f 
compute_KEW_positionO; 

rotate  (aim  .thetax, thetay);  /*  Update  Missile  orientation  */ 

rel_v  ector(xo1xin,aim  jnr); 


fpajmapO; 

hostio[hostioPtr}.pixel_x  =  pixel_x; 
hostio[hostioPtr].pixel_y  =  pixel_y; 
hostio[hostioPtr]. radius  =  radius; 
for(i=0;i<=2;i++)  { 

hostio[hostioPtr3.missile_coord[i]  =  xm[i]; 
hostio{hostioPtrj.threat_cootd[i]  =  xo[i]; 


} 

hostio[hostioPtr].total_intensity  =  totaljntensity; 

distance  =  sqrt(  (double)  (mr[0]*mr[0]  +  mr[l]*mr[l]  +  mr[2]*mr[2])); 

CentroidData  [CentroidDataPtr]  .iAcoorX 
=  CentroidData[0].iIcoorX  =  pixel_x; 

CentroidData  [CentroidDataPtr].  iAcoorY  = 

CentroidData[0].iIcoorY  =  pixel_y; 

CentroidData  [CentroidDataPtr].  c  Area  =  M_PI  *  radius*  radius; 

if  (CentroidData[CentroidDataPtr].cArca  <  1)  CentroidDaU[CentroidDataPtr].cArea  =  1.0; 
Centroid  Data  [CentroidDataPtr].cLitensity  = 

total_intensity  *  ((  1  >  CentroidData[0].cArea  )  ?  1  :  CentroidData [0].c Area); 
CentroidData  [CentroidDataPtr].  range  =  distance; 

Centroid  Data  [++Centroid  DataPtr]  .c  Area=0, 

if(distance  >  previous_distance) 

hostio[hostioPtr].miss_distance  =  compute_miss_distance(); 
prcvious_distance  =  distance; 
hostio[hostioPtr++].  range  =  distance; 

fprintf(outfp  ,”pixel_x  =  %d\n”,  pixel_x); 
fprintf(outfJ>  "pixel_y  =  %d\n",pixel_y); 
fJ)rintf(outfJ)  "radius  =  radius); 

fprintf(outfp,"xm[0]  =  %f  W\xm[0]); 
fprintf(outfp  ”xm[l]  =  %fV^m[lj); 
fprintf(outfp,”xm  [2]  =  %fV>[2]); 

fprintf(outfp  ”xo[0]  =  %f  W\xo[0]); 

fprintf(outf^>  ”xo[l]  =  %f\n’\xo[l]); 

fprintf(outfJ>  ,”xo[2]  =  %f  W’,xo[2j); 

fprintf(outf{), "range  =  %f  W\ distance); 

fprintf(outfp, "intensity  =  %d  W,CentroidData[0].cIntensity); 

fflush(outfp); 


} 
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computc_KHW_posidonO 

{ 


float  rj 3^xtgy,gz,ar(2],«m[3]lz; 
z  =  xm[2]  +  Z_OFFSET; 

r  =  sqit(  (double)  ( xm[0J*xm[0]  +  xm[l]*xm[l]  +  z*z) ); 
i3  =  (GRAV  * EarthMass) /(r*r*r); 

gx  =  -xm[0]  *  r3; 
gy  =  -xm[l]  *  r3; 
gz  =  -z  *  r3; 

fjprintf(mfp"delta  jt  =  9&f\n",Control.delta_x); 
fprintf(infp”delta_y  =  %f\n",ControLdella_y); 
fflush(infp); 


ar(0]  »  Conlrol.delta_x  *  time_step; 
ar[l]  =  Control. delta_y  *  time_step; 

am[0]  =  aim[0][0]  *  tr[0]  +  aim[0][l]  *  ir[l] 
am[l]  =  aim[l][0]  *  ar{0]  +  aim[l][l]  *  ar[l] 
am [2]  =  aim[2][0]  *  aifO]  +  aim[2][l]  *  ar[l] 


vm[0]  +=  (am[0]+  gx)  *  time_step; 
vm(lj  +=  (am[l]+  gy)  *  time_step; 
vm[2]  +=  (am[2]+  gz)  *  time_stcp; 


xm[0]  +=  vm[OJ  *  lime_step; 
xmflj  +=  vm[l  j  *  time_step; 
xm(2]  +=  vm[2]  *  time_step; 


/*  r  =  axb  */ 
croM_prod(rfatb) 
float  r(31,al31,b(3]; 

( 

rl01  =  a[l]*b(2)-a[21*b[ll; 
r(ll  -  *[2]*b[0]  -  a[0]*b[2J; 
*f2J  =  a[0]*b{l]  -  a[lj*b[0); 


/*  Given  thetax,  thetay,  calculate  new  position  Orientation  matrix  A  */ 

rotate(A,thetaxttheiay) 

float  A[3]  [3  ],thetax, thetay; 

{ 

float  p{3],q[3]  j[3],pmag,qmag,rmag; 
inti; 

/*  Calculate  rotation  about  x  axis  */ 
for  (i=0,i<3;i++)  { 

p[i]  -  A[i][0J  +  A[i][2]*thetay; 

<j[i]  =  A[i][l]  +  A[i][2]+thetax; 

} 

/*  force  orthogonal  coordinate  system  */ 

cross_prod(r,p,q);  /*  r=pxq  */ 

cross_prod(q,rfp);  /*  q=rxp  ♦/ 

f*  Force  vectors  to  be  unit  length  1  */ 

pmag ss  sqit(  (double)  (pl0]*p[0]  +  p[l]*p[lj  +  p[2]*p[2]) ); 

qmag  =  sqit(  (double)  (  q[0]*q[0]  +  q[l]*q[13  +  q[2]*q[2])  ); 

imag  =  sqrt(  (double)  (  r[0]*r{0]  +  rfl]*rfl]  +  r[2]*r(2]) ); 

for(i=Od<3  *4++)  { 

A[i][0]  =  p[i]/pmag;  /*  pOqOrO  */ 
A[i][l]  =  q[ij/qmag;  /*  A  =  pi  ql  rl  */ 
A[i][2]  =  ifi)/rmag;  j*  p2q2r2  */ 


36 


} 


I*  User  Cramers  rule  to  solve ,  assumes  det  of  matrix  si*/ 

solve(A,x,b) 

float  A[3][31,b[3],x[3]; 

( 


x[0]  =  ( 

b[0]  *  (  A[1][1]*A[2][2]  -  A[2][1]*A[1][2] ) 

-  A[0][1]  *  (b[lj  *A[2][2]  -  A[l][2]*b[2]  ) 
+  A[0][2]  *  (  bfl]  *A[2][1]  -  A[l][l]*b[2]  ) 
); 


x[l]  =  ( 

A[0][0]*(b[l]  *A[2][2]  -  b[2]  *A[1][2] ) 
-b[0]  *  (  A[1][0]*A[2][2]  -A{1][2]*A[2][0] ) 
+  A[0][2]  *  (  A[1][0]*  b{2]  -  b[l]  *A[2][0]) 
): 


x[2]  =  ( 

A[0][0]  *  ( A[l][l]*b[2]  -  A[2][l]*b[l] ) 

-  A[0][1  J  *  (  A[l][0]*b[2]  -b[ll  *A[2][0] ) 

+  b[0]  *  (  A[1][0]*A[2][1]  -  A[1][11*A[2][0J ) 

); 


} 

/*  compute  current  position  and  velocity  */ 

ballistic(x,v,tnow) 

float  *x,*v,mow; 

{ 


float  r,r3,gx,gy,gz,z; 
z  =  x[2]  +  Z_OFFSET; 

r  =  sqtt((douHe)  (x[0]*x[0]  +  x[l]*x[l]  +  z*z )); 
r3  =  (GRAV  *  EaithMass)  /  (  r  *  r  *  r ); 

gx  =  -x[0]  *  r3; 
gy  =  -x[l]*r3; 
gz  =  -z  *  r3; 

v[0]  +=  gx  *  time_step; 
v[l]  +=  gy  *  time_step; 
v[2]  +=  gz  *  time_step; 

x[0]  +=  v[0]  *  lime_step; 
x[l]  +=  v[lj  *  time.step; 
x[2]  +=  v[2]  *  time_step; 


/*  Catch  Math  Errors  ,  this  is  done  to  prevent  atan2  from 
complaining  about  0  */ 
int  matherr(exc) 
struct  exception  *exc; 

{ 

retum(l); 

} 

/* 

initialize  these  things 

inidalize_threat  object(aim,  intense,  xo,  vo,  size,  time_step,  t ); 
*/ 

initialize_threat_object() 

{ 


int  Lj; 
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if (NUIX=^infp^openOhreat_obj. input,  dump’’/^’’)))  { 

fprmtf(stderr,Xould  not  open  thrcat_obj  .mpuLdumpW’) ; 
cxit(l); 

} 

if(NUlX^(outfj^openC^reat_obj\outpuLdump’7,w”)))  { 
fprintf(stdcrr,Xould  not  open  threat  obj.outpuLdumpW'); 
cxiKl); 

) 

for(i=0u<=2;i++)  { 

for(j=0*j<=2;j++)  aim[i][j]  =  initialire.missile_aim[i][j]; 

xolij  =  initialize.thrcat_coor[i]; 

vo[ij  =  inilialire.threat_velocity[i3; 

xm[i]  =  initialize.missile_coor[i]; 

vm[ij  =  initialize.missile_velocity[i]; 

} 

sire  =  initialize.threat_size; 

intense  =  initialize.threat Jntensity; 
txme.step  =  initialize.time_step; 

1  =  0; 

forCi=0*4<=2a++) 

for(f=0*j<=2y++) 

fprintf (infp  tMaim[%d]  [%d]  =  %fW\ij,aim[i]lj]); 
for0=0u<=24++)  fprintf(infi>  ”xo[%d]  %fW\i,xo[i]); 
foifi=04<=24++)  fprintf(infp,”vo[%d]  %NT\i,voli]); 
for(i=0*ti<=2;i++)  fprintf(infp,”xm[%d]  %fWJ,xm[i]); 
for<i=0*4<=2;i++)  fprintf(infp,”vm[%d)  %fNn’J,vm[i]); 
fjprintf(infp,Msize  =  %fV\size); 
fprintf(infp, "intensity  =  %fW' .intense); 
fprintf(infp,”time_step  =  %fNnV\time_step); 

hostio[0].miss_distance  =  -1.0; 
preview  s_disunce  =  2e30; 
distance  =  le30; 


} 


fpa^mapO 

l 

intij; 

f*  Calculate  which  pixels  are  actually  turned  on 
see  note  at  top  for  weird  stuff  in  y  on  missile 
relative  coordinate  system  */ 
if(mr[2]<0)  { 

pixeLx  =  (  (ArraySize/2)  -0.5+  PXEL_PER_RAD  *  atan((double)  (mr[0]/(-mr{2]) )) ); 
pixels  =  ( (ArraySire/2)  -0.5  +  PXEL_PER_RAD  *  atan((double)  (mr[l]/(-mr[2]) )) ); 

if  ((mr(2]>0)  ll(pixeljt><AnaySize-l)) 

il  (pixel_x<0)  II  (pixel_y>(ArraySize-l))  II  (pixel_y<0))  { 

.  pixel_x  =  -l; 
pixel_y  =  -1; 

} 


/*  Here  is  where  we  figure  out  how  big  it  is.  and  how  bright. 

Intensity  goes  as  inverse  r  squared  */ 
radius  =  (int)  (atan((douMe)  (size/mr[2])  )  *  PXEL_PER_RAD ); 
if  (radius  <0)  radius  =  -  radius; 
totaljn  tensity  =  ( intense  /  (mrf2]  *  mr[2])); 
if  (radius>l)  total_in  tensity  =  total_in  tensity  /  (radius*  radius); 
if  (total  Jntensity  <  2  )  total  Jntensity  =  1; 
if  (total_in  tensity  >  63  )  total  Jntensity  =  63; 


} 


/*  finds  the  relative  vector  from  xb  to  xa,  then  transforms 
by  the  inverse  of  the  rotation  matrix  rot. 
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Result  is  put  in  res,  note  that  all  of  these 
must  be  declared  1  *f 
rcl  vector(xa^b,rot,res) 

float  xa[3],xb[3],rot[3][3]fres[3]; 

{ 

float  d[3]; 

d[0]  =  (xa[0]~xb[0]); 
d[13  =  (xa[13-xb[l]); 
d[23  =  (xa[23-xb[2]); 

solve(rot,res,d); 

} 


transpose(A3) 
float  A[3][3]3t3][3]; 

{ 

intij; 

for(i=0u<=2u++) 

for(j=0*j<=2;j++) 

B[j][i]  =  A[i][j]; 

} 

float  compute_miss_distanceO 

{ 

float  t,dt  =  .00001,  dx[3],dist=le30,prev_dist=2e30; 
int  i; 

while(prev_dist  >  dist )  { 
prcv_dist  =  dist; 
forfl=0^<=2^++)  { 
xm[i]  =  xm[i]  -  vm[i]  *  dt; 
xo[i]  =  xofij  -  vo[i]  *  dt; 

} 

for^=0-4<=2;i++)  { 
dx[i3  =  xm[i]  -  xo[i3; 
dx[i3  =  dx[i]*dx[i]; 

} 

dist  =  sqit(  (double)  ( dx[03  +  dx[l]  +  dx[23 ) ); 

} 

return  (prev_dist); 

} 


9.2.6  TVacking  Algorithm  trackx 

#include  “track  Ji” 

#define  MaxTracks  32 
#define  MaxTracksPl  33 
#define  MaxMiss  4 

#deflne  NumCorWeight  0.1  /*  used  to  determine  target  selection  ♦/ 

#define  AreaWeight  0.4 

#deflne  IntensityWeight  0.5 

#define  RAD.PER  PXEL75.0e-6 

#define  TRUE  -1 

#define  FALSE  0 

extern  int  CentroidDataPtr, 

extern  struct  Centroid  DataType  CentroidData[3; 

extern  structControlType  Control; 

extern  structinitializeType  initialize; 

/*  program  object_tracking; 

{  -  receives  cluster  data  from  clustering  chip. 

-  predicts  the  movement  of  each  cluster  in  tfEstimateVx  and  tfEstimateVy 

-  receive  updated  orientation  of  the  FPA  in  delta_theta  and  delta_phi  } 

V 


struct  TCtype  { 

int  tcTrackNum, 
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tcPriority; 
float  tcRPriority, 

tcPositionX, 

tcPositionY, 

tcWeightX, 

tcWdghtY, 

tcMin  Distance, 

tcNumCor, 

tcCcnlD, 

tcCcnAcoorX, 

IcCcnAcoorY, 

tcCcnlcoorX, 

tcCcnlcoorY, 

tcRange; 

int  tcCenArea; 

long  tcCcnln tensity; 


stnictTFtype  { 

int  tfTradcID, 
tfNumCor, 
tfNumMiss; 
float  tfEstimateX, 

tfEstunateVX, 
tfXPll, 
tfXP12, 
tfXP22, 
tfEstimateY , 
tfEstimateVY, 
tfYPll, 
tfYP12, 
tfYP22, 
tfArca, 

tfCenAcoorX, 
tfCenAcoorY, 
tfCenlcoorX, 
tfCenlcoorY, 
tfRange; 

long  tflntensity; 

int  tfCSO, 

); 

struct  Centroidtype  { 
int  cID; 

float  cAcoorXt 

cAcoorY, 
dcoorX, 
cIcoorY, 
cRange; 
int  cAtea; 

long  clntensity; 

}; 

static  float  time; 
static  struct  TCtype 
static  stnictTFtype 
static  stnictCentzoidtype  centroid; 

staticint  LastTrack,  LastNew, 

TrackID,  CentroidID, 
ptr,  lowptr, 

static  float  dx,  dy,  dist2,  priority,  lowpriority,  dt,  dt2,  dtsqr, 
static  float  x_dot>y_dot; 

staticint  NotMatchCentroid,  LastCcntroid,  iterations; 

static  float  RadPix2,  Pixels  PerRadian,  ProcessNoise,  MeasurementNoise; 
static  float  initialPll,  inidalP12,  initialP22t  PixelOffset; 
static  float  Targetlntensity ; 

staticint  TaigetTrack  ,  frame  ,  error,  TargetSelected ; 
static  float  deltajheta,  delta_phi,  theta_dot,  phi_dot,  threat_intensity; 

static  float  range; 
static  floatMaxDist; 
static  floatprevious_x, 
previously; 


TC[MaxTracksPl]; 

TF[MaxTracks]; 
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static  float  Num_obj; 

static  int  FinalAcquisition;  f*  On  Ada  this  is  supposed  to  be  boolean  type  */ 
static  AoatTaigetSelectionTime; 

static  flo^tTrackingRange;  /*  do  not  apply  angular  correction  if  inside  this  range.  ( in  radians )  */ 
static  floatFinalAcquisitionRange; 

#ifdef  DUMP 
FILE  *infp,*oUtfp; 

#endif 

track_objectO 

{ 

float  xdot_range_comp, 

ydot_range_comp, 

a; 

ddta_theta  =  Control.delta_theta; 
delta_phi  =  Control. delta_phi; 

#ifdef  DUMP 

fprintf (infp  ,”delta_theta :  %fW\delta_theta); 
fprintf(infpf**delta_phi:  %Ni’\delta_phi); 

#endif 

range  =  TFlTargetTrack]  .tfRange; 
if  (range  >  80e3)  MaxDist  =  6; 
else  MaxDist  =  (80e3/range)*6; 

PerformTrackingO; 

frame  =  frame  +  1; 
time  =  time  +  dt; 

if  (time>=  TargetSelectionTime)  { 
if  (ITargetSelected)  { 

SdectTargetO; 

phLdot  =  TF[TaigetTrack]  .tfEstimateX  -  0.5*RAD_PER_PXEL; 
theta_dot  =  TFlTargetTrack]  .tfEstimate  Y  -  0.5*RAD_PER_PXEL; 
x_dot  =  range*tan((double)  TFlTargetTrack]. tfEstimate  VX)/dt; 
y  dot  =  range*tan((double)  TFlTargetTrack]  .tfEstimate VY)/dt; 

} 

else  { 

if  ((range  <  FinaLAcquisitionRange)  &&  ((TFlTargetTrack] .tfNumMiss  >  0)  II  FinalAcquisition))  { 
FinalAcquisition  =  TRUE; 

FindTaigetO; 

range  =  TFlTargetTrack]  .tfRange; 

x_dot  =  range*tan((double)  TF  [TargetTrack]. tfEstimateX— 0.5*RAD_PER_PXEL)/dt; 
y_dot  =  range*tan((double)  TFlTargetTrack], tfEstimate  Y—0.5*RAD_PER_PXEL)/dt; 
phijdot  =  TF[TaigetTrack].tfEstimateX  -  0.5*RAD_PER_PXEL; 
theta_dot  =  TFlTargetTrack]. tfEstimate  Y  -  0.5*RAD_PER_PXEL; 

} 

else 

{ 

if  (TF[TaigetTrack].tfNumMiss  >  0)  { 

FindTargetO; 

range  =  TFlTargetTrack].  tfRange; 

if  ((TF[TargetTrack].tfEstimateX>TrackingRange)  II  (TF{TargetTrack].tfEstimateX<-TrackingRange))  { 
phi  dot  =  TFlTargetTrack].  tfEstimateX  -  0.5*RAD_PER_PXEL; 

} 

else  { 

phi_dot  =  0; 

if  ((TF[TargetTrack].tfEstimateY>TrackingRange)  II  (TF[TargetTrack].tfEstimateY<-TrackingRange))  ( 
theta  dot  =  TFlTargetTrack]. tfEstimate Y  -  0.5*RAD_PER_PXEL; 

} 

else  { 

thetajdot  =  0; 

) 

x_dot  =  range*tan((double)  TFlTargetTrack].  tfEstimate  VX)/dt; 
y  dot  =  range  *tan((double)  TF[TaigetTrack].tfEstimateVY)/dt; 

) 

previous_x  =  TFITaigetTrack].  tfEstimateX; 
previously  =  TF[TargetTrackj.tfEstimateY; 


) 


} 

ControLx^dot  =  x_dot; 

ControLy_dot  =  y_dot; 

.  ControLphi_dot  =  phi_dol; 

Conlrol.thcu_dot  =  theta_dot; 

#ifdcf  DUMP 

fpnnif(outfp,MX_dot:  %f\nMrx_dot); 
fprintf(outfp,"Y_doi:  %f\n*\y_dot); 
fprintf(outfp,”Phi_dot:  %fSn”tphi_dot); 
fjprintf(outfj)tMTheujdot:  %fW\theta_dot); 

#endif 

} 

r - -  - — . . =========== . — . ♦/ 

KalmanTimeUpdate(pofition,velocity,P  1 1  *P12,P22) 
float  ♦positional  1  ,*P12>*P22,*vclodty; 

{ 

♦position  =  ♦position  +  (dt  *  ♦velocity); 

♦Pll  =  *P11  +  ((dt2  *  *P12)  +  (dtsqr  *  *P22)); 

♦P12  =  ♦P12  +  (dt**P22); 

♦P22  =  *P22  +  ProcessNoise; 

) 

/♦========== .  - .  . - .  ==^-- . 

procedure  KalmanMeasurementUpdate  ( MeasuredPosition  :  real; 
var  position,  velocity,  PI  1 ,  P12,  P22 :  real );  ♦/ 

KaImanMeasurementUpdale(Measured  Position  .position  ,velodty,P  1 1  ,P12,P22) 
float  *MeasuredPosition,*position,*velocity,*Pl  1  ,*P12,*P22; 

float  temp,  Kl,  K2,  StateError; 


StateEiror  =  ♦MeasuredPosition  -  ♦position; 
temp  =  1.0  /  (*P1 1  +  Measu  remen  tNoise); 
Kl  =temp  ♦  ^Pll; 

K2  =  temp  ♦  *P12; 
temp  =  *P12; 

♦Pll  =  *Pll  ♦  (1.0 -Kl); 

♦P12  =  *P12  ♦  (1.0  -  Kl) ; 

♦P22  =  *P22  -  (K2  *  temp); 

♦position  =  ♦position  +  (Kl  ♦  StateError); 
♦velocity  =  ♦velocity  +  (K2  ♦  StateError); 


/♦= .  ===== .  = . r . = . = 

procedure  Initialize;  ♦/ 

initialize  track  objectO 

{ 

inti; 

#ifdef  DUMP 

if  (NULL  =(infp=(fopenC,Track_obj.inpuLdump”,Mw”))))  ( 
fprintf(stderTfXouldn*t  open  Track_obj.input.dumpW*); 
exit(l); 

) 

if  (NULL  =(outfp=(fopenCTrack_obj.outpuLdump,,,”w,r))))  { 
fprintf(stderr,”couldn’t  open  Track_obj.outpuLdump\n’0; 
exit(l); 

} 

#endif 

/*  Get  Time  stamp  from  master  initialization  ♦/ 
dt  =  imdalize.time.step; 
dt2  as  2.0  *  dt; 
dtsqr  =  dt  ♦  dt; 

FinalAcquisition  =  FALSE; 
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RadPbc2  =  RAD  PER.PXEL  *  RAD_PER_PXEL; 

PixdsPerRadian  =  1.67  RAD_PER_PXEL; 

ProcessNoisc  =  2.5  *  RadPix2; 

MeasurementNoise  =  1.0*RadPix2; 

initialPll  =  2.0*RadPix2; 
initialP12  =  0.0  *  RadPix2; 
initialP2i2  =  100.0  *  RadPix2; 

PixelOffset  =  AnaySize  /  2.0  -  0.5; 

LastTrack  =  0; 

LastNew  =  0; 

for  (i=0;i<=MaxTracks;i++)  { 

TC[i].tcTrackNum  =  i; 

TC[ij.tcPriority  =  0; 

} 

TrackID  =  0; 

TargetSelccted  =  FALSE; 

time  =  0; 

x jdot  =  0; 

y_dot  =  0; 

lheta_dot=0; 

phi_dot=0; 

frame  =  0; 
error  =  FALSE; 

threat_in  tensity  =  initialize.threat_intensity; 

TaigetSelectionTime  =  initialize.  target_select_time; 

Nuni_obj  =  initialize.Num_obj; 

TrackingRange  =  initialize.TrackingRange*RAD_PER_PXEL; 
FinalAcquisitionRange  =  initialize.FinalAcquisitionRange; 

#ifdef  DUMP 

fprintf(infp”threat_in  tensity:  %f  W’.thrcatjn  tensity); 
fprintf(infp,”time_step:  %f\n*\dt); 

fprintfCinfp/TargetSelecdonTime:  %i  Nn’\TaigetSelectionTinie); 
fprintf(infp,,,Num_obj :  %d  V* Jfum_obj); 

fprintf(infp/’FinalAcquistionRange:  %fW\FinalAcquisitionRange); 
fprintf(infp,’TrackingRange:  %d  \n\n’\  initialize.TrackingRange); 

#endif 

} 

I*  . - . .  =  .  ",a= 

Procedure  Compensate_FPA_Rotation;  */ 

Compensate_FPA JRotation  0 

{ 

inti; 

for(i=0;i<=(LastTrack  -  l);i++)  { 

TF[TC[i].tcTrackNum]  .tfEstimateX  -=  delta_phi; 
TF[TC[i].tcTrackNum].tfEstimateY  -=  delta jheta; 

} 

} 

I*  =  '  '  ■  ■■  =  ====== 

procedure  UpdateTracks;  */ 

UpdateTracksO 

{ 

inti; 

for(i=0;i<=(LastTrack  -  l);i++)  { 

/*  check  for  multiple  centroid  associations  */ 
if  ((TC [i] .tcN umCor  >  0)  &&  (TC[i].tcCenID  >=  0) )  { 
ptr  =  i+  1; 

while  (ptr  <  LastTrack)  { 

if  ((TC[i].tcCenID  =  TC[  ptr  ].tcCenID) ) 

if  ((TC[i].tcMinDistance  >  TC[  ptr  ].tcMinDistance)  ) 
TC[i].tcCenID  =  -1; 

else 

TC[  ptr  J.tcCenID  =  -1; 
ptr  =  ptr  +  1; 
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} 


} 

if  ((TC[i].tcNumCor  =  0)  )  f*  no  correlation  with  track  file  *1 

if  ((TF[TC{i].tcTrackNum].tfNumMiss  >=  MaxMiss)  ) 
TC[i].tcPriority  =  0; 

else  { 

TF[TC[i]  .tcTrackNum]  .tfNumMis  s++; 
TC[i].tcPriority  =  TF[TC[i].tcTrackNum].tfNumCor 
-  TF[TC[i].  tcTrackNum].  tfNumMis  s; 

} 

else  if  ( (TC[i].tcCenID  =  (-1))  ) 

/*  correlation  but  another  track  file  is  closer  */ 


( 

TF[TC[i]  .tcTrackNum]  .tfNumMiss  =  0, 

TC[i].tcPrioiity  =  TF[TC[i].tcTrackNum].tfNumCor, 
KalmanMeasurementUpdate(  &(TC[i].tcPositionX), 

&(TF[TC[i].tcTrackNum].tfEstimateX), 
A(TFlTCiij.  tcTrackNum].  tfEstimateVX), 
&(TF[TC[i].tcTrackNuml.tfXPl  1), 
&(TFITC[i].tcTrackNum].tfXP  1 2), 
&(TF[TC[i]. tcTrackNum]. tfXP22)  ); 
KalmanMeasurementUpdate(  &(TC[i].tcPositionY), 

A(TFlTC[i].tcTrackNum].tfEsdmateY), 
&(TF[TC[i].tcTrackNum].tfEstimateVY), 
&(TFITC[i].tcTrackNum].tfYPll)t 
&(TFlTC[i] .  tcTrackNum  j.tfYP  1 2), 
&(TFlTC[i].tcTrackNum].tfYP22) ); 

}  else  {  f*  update  from  correlated  centroid  */ 
TF[TC[i].tcTrackNum}.tfNumCor++; 
TF[TC[ii.tcTrackNumi.tfNumMiss  =  0, 

TC[i].tcPriority  =  TF[TC(i].tcTrackNum].tfNumCor, 

TF[TC[i].  tcTrackNum].  tfln  tensity  =  TC[i].tcCcnIn  tensity; 
TF[TCti].  tcTrackNum]. tf Area  =  TC[i].tcCenArca; 

TF(TC[i].  tcTrackNum]  .tfRange  »  TC[i].tcRange; 

TF[TC(i].  tcTrackNum  j.tfCen  A  coorX  =  TC[i].tcCcnAcoorX; 
TF[TC  [ij  .tcTrackNum]  .tfCen  Acoor  Y  =  TCfij.tcCenAcoorY; 
TF[TC[ij. tcTrackNum]. tfCenlcoorX  =  TC[i].tcCenIcoorX; 
TF[TC[i].tcTrackNum]  .tfCenlcoorY  =  TC[i].tcCenIcoorY; 
KalmanMeasu  rementU pda te(  &(TC[i].tcCenAcoorX), 

&(TF{TC[i].tcTrackNum].tfEstimateX), 
&(TFlTC[i].tcTrackNum].tfEstimateVX), 
A(TFlTC[i].tcTrackNum].tfXP  1 1 ). 
&(TF[TCii].  tcTrackNum]. tfXP  12), 
&(TF[TC[i].tcTrackNum].tfXP22)  ); 
KalmanMeasurementUpdate(  &(TC[i].tcCenAcoorY), 

&(TFlTC[i]  .tcTrackNum]  .tfEstimateY), 
&(TF[TC[i]  .tcTrackNum]  .tfEstimateVY), 
&(TF[TC[i].tcTrackNum].tfYPll), 
&(TFrrC[i].tcTrackNum].tfYP12), 
&(TF[TC[i].tcTrackNum].tfYP22) ); 


!* . .  . .  = .  = 

procedure  InitializeNewTracks;  */ 

InitializeN  ewTracksO 

{ 

int  i; 

for  (i=LastTrack;i<=(LastNew  -  l);i++)  { 

TF[TC[i]. tcTrackNum  ].tfTrackID  =  TrackID; 

TrackID++; 

TF[TC[i].tcTrackNum].tfNumCor  =  1; 

TF(TC[ij.  tcTrackNum].  tfNumMiss  =  O, 

TCpj.tcPriority  =  1; 

TF[TC[i].tcTrackNum].tfIn  tensity  =  TC[i].tcCenIn  tensity; 
TF[TC  [i]  .tcTrackNum] .  tf  Area  =  TC[i].tcCenArea; 
TF[TC[i].tcTrackNum].  tfRange  =  TC[i].tcRange; 
TF[TC[i].tcTrackNum].tfEstimateX  =  TC{i].tcCenAcoorX; 
TF[TC[i] . tcTrackNum]. tfEstimateVX  =  0.0; 

TF  [TC[i]. tcTrackNum].  tfXP  11  =  initialPll; 

TF[TC[i] . tcTrackNum]. tfXP  12  =  initiaiP12; 
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TF[TC[i].tcTrackNum}.tfXP22  =  inilialP22; 
TF[TC[i].tcTrackNumj.tfEstimateY  =  TC[ij.tcCenAcoorY; 
TF[TC[ii.tcTrackNumj.tfEstimaicVY  =  0.0; 

•  TF  [TC[ij. tcTrackNum]. tfYPll  =  initialPll; 
TF[TC[i].tcTrackNum].tfYP12  =  initialP12; 
TF[TC[i].tcTrackNum].tfYP22  =  initialP22; 

TF[TC[i]  .tcTrackNum].  tfCenAcoorX  =  TC[i].tcCenAcoorX; 
TFtTC[ii.tcTrackNumj.tfCenAcoorY  =  TC  [i]  .tcCenAcoor Y; 
TF[TC[i].tcTraclcNum].tfCcnIcoorX  =  TC[i] .tcCenlcoorX;  - 
TF[TC[i].  tcTrackNum].  tfCenlcoorY  =  TC[ij.tcCenIcoorY; 

} 

} 

I*  . ===== . ===== - ^^s=^-^=s;==== 

procedure  RankTracks;  *1 

RankTracksO 

{ 

int  ij.templ  ,temp2; 

if  ((LastNew  >  1))  /*  sort  on  priority  *J 

for  (i=0;i<=LastNew-2  u++)  { 
ptr=i; 

for  (j=(i+ 1 )  J<=(LastNew-l )J++) 

if  (TC[i].tcPriority  <  TC[j].tcPriority) 

ptr=j; 

if  (ptr  t=i)  { 

tempi  =  TC[ptr].tcTrackNum; 
temp2  =  TC[ptrj.tcPriority; 

TC  [ptr]  .tcTrackNum  =  TC[i].tcTrackNum; 
TC[ptr].tcPriority  =  TC[i].tcPriority; 

TC  [i]  .tcTrackNum  =  tempi; 

TC[i].tcPriority  =  temp2; 

} 

} 

LastTrack  =  0; 

while  (TC[LastTrack].tcPriority  >  0)  LastTrack  =  LastTrack  +  1; 

LastNew  =  LastTrack; 

J 

p9„mssss - ■,a,-BBaas=:  - - - 

procedure  SetUpCorrelation;  *1 

SetUpCorrelationO 

{ 

inti; 

for  (i=0;i<=(LastTrack  -  l);i++)  { 

KalmanTimeUpdate(  &TF[TC  [i].  tcTrackNum]  .tfEstimateX, 

&TF[TC[i]  .tcTrackNum]  .tfEstimateVX, 
&TF[TC  [i]  .tcTrackNum]  .tfXPl  1 , 

&TF  [TC  [i]  .tcTrackNum  ]  .tfXP12, 
&TF[TC[i]  .tcTrackNum]  .tfXP22 ); 
KalmanTimeUpdate(  &TF[TC[i].tcTrackNum].ifEstimateY, 

&TF[TC[i]  .tcTrackNum]  .tfEstimateV  Y, 
&TF[TC[i]  .tcTrackNum]  .tfYPll , 
&TF[TC[i].tcTrackNum].tfYP12, 
&TF[TC[i]  .tcTrackNum]  .tfYP22 ); 
TC[i].tcPositionX  =  TF[TC[i].tcTrackNum].tfHstimateX; 

TC[i].tcPositionY  =  TF[TC[ij. tcTrackNum].  tfEstimateY; 

TC[i].tcWeightX  =  1.0/ (TF[TC[i].tcTrackNum].tfXPl  1); 

TC[i].tcWeightY  =  1.0/ (TF[TC[i].tcTrackNum].tfYPl  1); 

TC[i].tcMinDistance  =  MaxDist; 

TCfij.tcNumCor  =  0; 

} 

) 

/»= - = - ■  -  ■ . ===== . ======= 

procedure  InputCentroid;  */ 

InputCentroidO 

{ 

float  iAcoorX,  iAcoorY,  ilcoorX,  ilcoorY ; 
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iAcoorX  =  CentroidData[CentroidDataPtr]iAcoorX; 

iAcoorY  =  CcntroidData[CcntroidDauPir].iAcoorY; 

ilcoorX  =  CentroidDaU[CentroidDauPlrj.iIooorX; 

flcoorY  =  CentradData[CentroidDataPtr).iIcoorY; 

centroid.  cArea  =  CentroidDiufCeniroidDataPtr]. cArea; 
centroidclntcnsity  =  CentroidData[CentioidDataPtr].cIntensity; 
centroid.  cRange  =  Centroid  Data  [Cent  roidDataPtr++]  .range; 

#ifdef  DUMP 

fprintf(infp  "ilcoorX :  %f\n",ikoorX); 
fprintfCmfpfMiIcoorY:  %NT,iIcoorY); 
fprintf (infp  "cArea :  centroid  c Area); 

fprintf(infp  "clntensity:  %dV\centioidcIntensity); 
fprintf(infp  "cRange:  %f\nW\centroid.cRange); 

#endif 

centroid.  cID  =  CentroidID; 

CentroidII>4*+; 

centioidcA  coorX  *  (iAcoorX  -  PixelOffset)  *  RAD_PER_PXHL; 
centroidcAcoorY  =  (iAcoorY  -  PixelOffset)  *  RAD .PER.PXEL; 
centroid. clcoorX  =  (ilcoorX  -  PixelOffset)  *  RAD_PER  PXEL; 
centroiddcoorY  =  (ilcoorY  -  PixelOffset)  *  RAD  PER  PXEL; 


procedure  PerformTracking;  */ 

PerformTrackingO 

(  ini  centroid_critr=0; 

Compehsate_FPA_RotationO; 

SetUpCorrelationO; 

LastCentroid  =  0; 

CentroidID  =  0; 

while  (cenlroid_cntr<Num_obj)  { 
centroid_cntr++; 

InputCentroidO; 

I*  compare  against  tracks  ♦/ 

NotMatch  Centroid  =  TRUE; 
ptr  =  0; 

while  (ptr  <  LastTrack)  { 

dx  =  centroid  cAcoorX  -  TC[ptr].tcPositionX; 
dy  =  centroid.cAcoorY  -  TC[ptr].tcPositionY; 
dist2  =  TC[ptr].tcWeightX*(dx*dx)  +  TC[ptr].tcWeightY*(dy*dy); 
if  (dist2  <  MaxDist)  {  /*  correlated  with  this  track  file  */ 

TC[ptr].tcNumCor  =  TCfptrl.tcNumCor  +  1; 

NotMatch  Centroid  =  FALSE; 
if  (dist2  <  TC [ptr]. tcMin Distance)  { 

TC[ptr].tcMinDistance  =  dist2; 

TC[ptr].tcCenID  s  centroid.cID; 

TCfptrj.tcCenAcoorX  =  centroid.  cAcoorX; 

TC{ptr].tcCenAcoorY  =  centra  Ac  AcoorY; 

TCjptrj.tcCenlcoorX  =  centroid,  cl  coorX; 

TC[ptr].tcCenIcoorY  =  centra  AcIcoorY; 

TC[ptr].icCenArea  =  centroid  cArea; 

TC[ptr].tcRange  =  centroid.  cRange; 

TC  [ptr].  tcCenln  tensity  =  centroid  (intensity; 

} 

} 

ptr++; 

} 

/*  compare  against  uncorrelated  centroids  */ 
if  (NotMatchCentroid  &&  (ptr  <  MaxTracks))  ( 

priority  =  centioidcAcoorX*centroid.cAcoorX 

+  cenlroid.cAcoorY*centroidcAcoorY; 
if  (LastNew  <  MaxTracks)  {  /*  room  to  start  new  track  *'/ 

TC[LastNew]  .tcRPriority  =  priority; 

TC[LastNew]  .tcCcnID  =  centroid.cID; 

TC[LastNew].tcCenAcoorX  =  centroid  cAcoorX; 

TC  [LastNew]  .tcCenA coot  Y  =  centroidcAcoorY; 

TC  [LastNew]  .tcCenlcoorX  =  centroid  cl  coorX; 

TC[LastNew].tcCenIcoorY  =  centroidcIcoorY; 

TC[LastNew].tcCenArea  =  centroid.cArca; 
TC[LastNew].tcRange  =  centroid.  cRange; 

TC  [LastNew]. tcCenlntensity  =  centroid  clntensity; 
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LastNew++; 

}  else  {  I*  check  other  new  trade  for  priority  */ 
lowpriority  =  -1.0; 
lowptr  =  LastNew; 
while  (ptr  <  MaxTracks)  { 

if  (lowpriority  <  TC[ptr].tcPriority)  { 
lowptr  =  ptr, 

lowpriority  =  TC[ptr].tcPriority; 

} 

ptr  =  ptr  +  1; 

} 

if  (priority  <  lowpriority)  ( 

TC  [lowptr]. tcRPriority  =  priority; 

TC  [lowptr]  .tcCenID  =  centroid.  cID; 

TC  [lowptr]  .tcCenAcoorX  =  centroid.  cA  coo rX; 
TC  [lowptr]  .tcCen  AcoorY  =  centroid.  cAcoorY; 
TC  [lowptr]  .tcCenlcoorX  =  centroid.cIcoorX; 
TC  [lowptr]  .tcCenlcoorY  =  centroiddcoorY; 
TC  [lowptr]  .tcCen  Area  =  centroid,  c  Area; 

TC  [lowptr]  .tcRange  =  centroid.cRange; 
TC[lowptr].tcCenIntensity  =  centroid,  (intensity; 

} 

} 

} 

} 

UpdateTracksO; 

InitializeNewTracksO; 

RankTracksO; 

} 


/  — - — - - 

procedure  SelectTarget;  */ 

SelectTargetO 

{ 

inti; 

float  maxNumCor,  maxArea,  maxlntensity.  Weight,  Target  Weight; 

maxNumCor  =  0; 
maxArea  =  0; 
maxlntensity  =  0; 

Taigetlntensity  =  0; 

f*  select  the  brightest  object  among  the  objects  that  meets  minimum 
number  of  Corelation  */ 
for  (i=0  ;i<=(LastTrack  -  l);i++)  ( 

if  (TF[i]  .tfNumCor  >  maxNumCor)  maxNumCor  =  TF[i]  .tfNumCor, 

if  (TFIil-tf Area  >  maxArea  )  maxArea  =  TF[i]  .tf Area; 

if  (TF[i].tfIntensity  >  maxlntensity ) 
maxlntensity  =  TF[i].tfIn  tensity; 

) 

TaigetWeight  =  0; 

TargetTrack  =  -1; 

for  (i=0,  i<=(LastTrack  -  l);i++)  ( 

Weight  =  (NumCorWeight*TF[i]  .tfNumCor/maxNumCor)  + 
(AreaWeight*TF[i]  .tf Area/max  Area)  + 

(Intensity  Weight*  TF[i]  .tflntensity/maxlntensity); 
if  (Weight  >  TargetWeight)  { 

TargetTrack  =  i; 

TargetWeight  =  Weight; 

} 

} 

if  (LastTrack  >  0  )  TargetSelected  =  TRUE; 


FindTargetO 
(  inti; 

Taigetlntensity  =  0; 

/*  select  the  brightest  object  among  the  objects  with  0  misses  */ 
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for  (i=0  u<s=(LasiTrack  - l);i++)  { 

if  ((TF[i].tfNumMiss  =  0)  &&  (TF[i].tfInlcnsily  >  Targctlntcnsity  ))  { 
Targetln  tensity  a  TF[i].tfIn  tensity; 

TargetTrack  =  i; 

} 

) 

) 


9.2.7  Guidance  los_gnc.c 

#indude  "track  J»” 

extern  struct  ControlType  Control; 
extern  struct  initial  izeType  initialize; 

static  float  angNavConst,  linNavConst,  thrusterjnterval, 
time_step,  thruster_tinief  max_divert; 

/*  compute  zero  mass  line  of  site  guidance  law  */ 
k»_gncO 
{ 


float  div; 

if  (thmiter_time  >=  thruster_interval)  { 

Controldelu_theta  -  angNavConst  *  Conlrol.theta_doi; 
ControLdelta_phi  =  angNavConst  *  Control.phi_dot; 

.  div  =  linNavConst  *  Control.x_dot; 
if  (div>max_diveit)  div  s  max^divert;  /*  Limit  divert  */ 
else  if  (div<  -max_divert)  div  =  -max_divert; 
ControLdeltajt  =  div; 

div  =  linNavConst  *  Contral.yjdot; 
if  (div>max_divert)  div  a  max_divert; 
else  if  (div<  -max_divert)  div  =  -max_divert; 
Control.delta_y  a  div; 

thruster jime  a  time_step; 


1 

else  { 


thruster  .time  +=  time_step; 
ControLdelu_x  =  0; 

ControLdelu_y  a  0, 

ControLdelta_theta  =  0; 
ControLdelu^phi  =  O, 

] 

} 

initialize Jos_gncO 

I 

angNavConst  =  initialize,  an  g_n  a  v_const; 
linNavConst  =  initialize  iin_nav_const; 
thrusterjnterval  a  initialize.thnisterjnterval; 
time_step  =  initialize.  time_step; 
thruster_time  =  thrusterjnterval; 
max_divert  =  initialize.max_divert; 

} 


9.3  Ada  Code 

9.3.1  HOST  interface  routine  hostio.ada 

#ifhdef  NET 
#defme  NET  net 
#endif 

procedure  hostio  is 
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fval :  float; 

—  for  inputing  floats  you  cant  input 

ival :  integer, 

—  for  inputing  integers  you  cant  input 

fov :  float; 

type  float_3  is  anay  (0 ..  2)  of  float; 
type  float J3x3  is  array  (0 ..  2, 0 ..  2)  of  float; 

type  f!oat_threats  is  array  (0  ..  3 1)  of  float; 
type  fioat_threatsx3  is  anay  (0 ..  31,0 ..  2)  of  float; 
type  integerjhreats  is  array  (0 ..  31)  of  integer, 

nearest_range, 
nearest jmiss_distancef 
panview_view, 
panviewjrange :  float; 

observation _point, 
missile_coord, 
missile_coordr :  float_3; 

pv_orient :  float_3x3; 

_range, 
miss_dis  Lance, 
prev_distance  :  float_threats; 

threat_coordr, 

threat_coord  :  float Jthreatsx3; 

KEW_x» 

KEW_y, 
frame_number, 
frame_update, 
hidden_frame_cntt 
Num_obj ;  integer; 

pixel_x, 
pttel_y, 
radius, 
total Jnt, 
thrcat_x, 

threat_y :  integerjhreats; 

—  pix_x,pix_y :  integer_threats; 
procedure  initialize_hostio  is 
begin 


ll_receive(host,  panview_view); 
U_receive(host,  panviewjrange); 

fov  :=  tan(panview_view  /  panview_range); 

ll_receive(host,  fval); 
obseivation_point(0)  :=  fval; 
ll_receive(host,  fval); 
observation_point(l)  :=  fval; 
ll_receive(host,  fval); 
observation_point(2)  :=  fval; 

ll_receive(host,  fval); 
pv_orient(0, 0)  :=  fval; 
lljirceiveChost,  fval); 
pv_orient(0, 1)  :=  fval; 

U_receive(host,  fval); 
pVjOrient(0, 2)  :=  fval; 
ll_receive(host,  fval); 
pv_orient(l,  0)  :=  fval; 

11  _receive(host,  fval); 
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pv_orient(l,  1)  :=  fval; 
n_rcccivc(host,  fval); 
pv_orient(l,  2)  :=  fval; 
Il_receive(host,  fval); 
pv_orient(2, 0)  :=  fval; 
Il_receive(host,  fval); 
pv_orient(2,  1)  :=  fval; 
H_receive(host,  fval); 
pv_orient(2,  2) fval; 

Num  jobj  :=  0; 

Il_receive(host,  Num_obj,  lsw); 

frame__update  :=0; 
ll_reccrvc(host,  framc_update,  lsw); 

frune_number  r=  O, 

end  initialize Jhostio; 


procedure  dumptohost  is 

i :  integer; 

begin 

—  Output  the  pan  view  data  for  the  KEW 
if  ((((0  <=  KEWjt  AND  KEW_x  <  1024) 

AND  0  <=  KEW_y)  AND  KEW_y  <512) 
AND  missile_cooidr(2)  <=  0.0)  then 

ll_send(host,  KEW_x, lsw); 

U_send(host1  512  -  KEW_y,  lsw); 
ll_send(host,  2,  lsw);  — Threat  type 

else 


ll_send(host,  -1 ,  lsw); 
ll_send(host,  -1,  lsw); 
H.sendChost,  -1 ,  lsw); 
end  if; 

Il_send(host>nearest_range); 

U_send(host>nearest_miss_distance); 

—  Send  out  the  the  objects  (targets) 
for  i  in  0 ..  Num_obj-l  loop 

ival  :=  pixel_x(i); 
ll_send(hostt  ival,  lsw); 
U_send(host,pix_x(20-i)Jlsw); 

ival  :=  pixel_y(i); 

Il_send(host,  ival,  bw); 
U_send(host4>ix_y(20-i),lsw); 

ival  :=  radius(i); 

U_send(host,  ival,  bw); 
ll_send(host,  totaI_int(i)); 


—  Output  the  pan  view  data  for  the  threat 
if  ((((0  <=  threat_x(i)  AND  threat_x(i)  <  1024) 

AND  0  <=  threat_y(i))  AND  threat_y(i)  <  5 12) 
AND  threat_coordr(i,  2)  <=  0.0)  then 

ll_send(host,  threat_x(i),  lsw); 
ll_send(host,  512  -  threat_y(i),  bw); 
ll_send(host,  3,  bw);  —  Threat  type 
lLsendfhost,  _range(i)); 
ll_send(host,  mbs_distance(i)); 

else 

ll_send(host,  -1,  bw); 
ll_send(host,  -1,  bw); 
ll_send(host,  -1,  lsw); 
ll_send(host,  -1.0); 
il_send(host,  -1.0); 
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end  if; 


end  loop;  —  for  i  in  0 ..  Num_obj-l 
.  ll_send(host,  frame_number,  lsw); 
end  dumptohost; 

procedure  dohostio  is 

j  :  integer, 
dummy :  integer; 


begin 

frame_number  :=  frame_number  +  1; 


j^O; 

while  j  <  Num_obj  loop 

ival  :=  0; 
fval  :=  7.0; 

ll_rcceive(NET,  ivaljsw); 
fval  :=  7.0; 
pixel_x(j)  :=  ival; 
pix_x(20-j)  :=  ival; 

ival  :=  0; 
fval  :=  7.0; 

11  receive(NET,  ivaljsw); 
tol  :=  7.0; 
pixeTy(j)  ival; 
pix_y(20~j)  *.=  ival; 


ival  :=  0; 

ll_receive(NET,  ival,  lsw); 
radius(j)  :=  ival; 

ll_receive(NHT,  ival); 
total_int(j)  :=  ival; 

ll_receive(NET,  fval); 
_range(j)  :=  fval; 

if  (j  =  0)  then 

U_receive(NET,  fval); 
missile_coord(0)  :=  fval; 
U_receive(NET,  fval); 
missile_coord(l)  :=  fval; 
ll_receive(NET,  fval); 
missile_coord(2)  :=  fval; 


missile_coord(0) 

:=  missile_coord(0)  -  observation_point(0); 
missile_coord(l) 

:=missile_coord(l)-observation_point(l); 

missile_coord(2) 

;=  missile_cooid(2)  -  observation _point(2); 

—  hio_solve(pv_orient,  missile_coordr,  missile_coord); 

—  User  Cramers  rule  to  solve,  assumes  det  of  matrix  :=  1 

missile_coordr(0)  := 

(missile_coord(0)  *  (pv_orient(l,  1)  *  pv_orient(2,  2) 

-  pv_orient(2, 1)  *  pv_orient(l,  2)) 

-  pv_orient(0,  1)  *  (missile_coord(l)  *  pv_orient(2, 2) 
-pv_orient(l,  2)  *  missile_coord(2)) 

+  pv_orient(0, 2)  *  (missile_coord(l)  *  pv_orient(2, 1) 

-  pv_orient(l,  1)  *  missile_coord(2)) 

); 


missile_coordr(l)  := 
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(pv_orient(0, 0)  *  (missilc_coord(l)  *  pv_orient(2, 2) 
-missile_coord(2)  *  pv_oricnt(l,  2» 

-  missilc_coord(0)  *  (pv_oricm(l ,  0)  *  pv_orient(2, 2) 
-pv_orient(l,  2)  *  pv_oricnt(2, 0)) 

+  pv_orient(0, 2)  *  (pv_orient(l,  0)  *  missile_coord(2) 

-  missile  coord(l)  *  pv_orient(2,  0)) 

X 


missile_coordr(2)  := 

(pv_orient(0, 0)  *  (pv_orient(l,  1)  *  missile_coord(2) 

-  pv_orient(2, 1)  *  missile_coord(l)) 

- pv_orient(0, 1)  *  (pv_orient(l,  0)  *  missile_cx>ord(2) 
-missile_coord(l)  *  pv_orient(2, 0)) 

+  missile_coord(0)  *  (pv_orient(l,  0)  *  pv_orient(2, 1) 

-  pvjment(l,  1)  *  pvjorient(2, 0)) 


KEW_x  :=  integer<511.5  +  1024.0  * 

iUn((missile_coordr(0)/(-missile_coordr(2))))  /  fov  ); 
KHW_y  :=  integer(255.5  +  1024.0  * 

atan((missile_coordr(l  )/(-mis  sile_coordr(2))))  /  fov  ); 


end  if; 


H_receive(NET,  fval); 
threat_coord(j,  0)  .*=  fval; 

Ii_rcceive(NET,  fval); 
thrcat_coord(j,  1)  :=  fval; 
n_recrive(NET,  fval); 
thrcat_coord(j,  2)  :=  fval; 

thrcat_coord(j,  0) 

?=  lhreat_coord(j,  0)  -  observation_point(0); 
threat_coord(j,  1) 

:=  threat_coord(j,  1 )  -  observation_point(  1 ); 
threat_coofd(j,  2) 

.*=  lhreat_coord(j,  2)  -  observation_point(2); 

—  hio_solve(pv_orient,  threat_coordr,  lhreat_coord,  j); 

—  User  Cramers  rule  to  solve,  assumes  det  of  matrix  :=  1 


lhreat_coordi<j,  0)  := 

(threat_cootd(j,  0)  *  (pv_orient(l,  1)  *  pv_orient(2, 2) 

-  pv_orient(2f  1)  *  pv_oriem(l,  2)) 

-  pv_orient(0, 1)  *  (threat_coord(j,  1)  *  pv_orieni(2, 2) 

-  pv_orient(l ,  2)  *  thrcat_coord(jt  2)) 
+  pv_orient(0, 2)  *  (threat_coord(jt  1)  *  pv_orient(2, 1) 

-pv_orient(l,  1)  *  threat_coord(j,  2)) 


); 


threat_coordr(j,  1)  .*= 

(pv_orient(0, 0)  *  (ihreat_coord(j,  1)  *  pv_orient(2,  2) 

-  threat_coord(j,  2)  *  pv_orient(l,  2) ) 
- threat_coord(j,  0)  *  (pv_orient(l,  0)*pv_orient(2, 2) 

-  pv_orient(l ,  2)  *  pv_orient(2, 0)) 

+  pv_orient(0, 2)  *  (pv_orient(l,  0)  *  thrcat_coord(j,  2) 

-  threat_coord(j,  1)  *  pv_orient(2, 0) ) 


threat_coordr(j,  2)  := 

(pv_orient(0, 0)  *  (pv_oricnt(l,  1)  *  threat_coord(j,  2) 

-pv_orient(2,  l)*threat_coord(j,  1)) 
-  pv_oriem(0, 1)  *  (pv_orient(l,  0)  *  thrcat_coord(j,  2) 

-  threat_coord(j,  1)  *  pv_orient(2, 0)) 
+  thrcat_coord(j,  0)  *  (pv_orient(l,  0)  *  pv_orient(2, 1) 

-  pv_oricnt(l,  1)  *  pv_orient(2, 0)) 


ll_receive(NET,  fval); 
miss_distance(j)  :=  fval; 
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threat_x(j)  :=  integer<51 1.5  +  1024.0  * 

atan(threat_coordr(j ,  0)/(-threat_coordr(j,  2))) 
/fov); 

.  thrcat_y(j)  :=  integer{255.5  +  1024.0  * 

atan (threat_coordr(j ,  l)/(-threat_coordr(j,  2))) 
/fov); 

j-j  +  i; 

end  loop;  —  while  j  <  Numobj 
—  find  nearest  range  and  miss  distance  for  display  on  host 

nearest_mis  s_distance  :=  1.0e20; 

nearest_range  :=  1.0e20; 

for  k  in  0.-Num_obj-1  loop 

if  (miss_distance(k)<nearest_nuss_di  stance)  then 
nearest_miss_distance  :=  missjdistance(k); 
end  if; 

if  (_range(k>cnearest_range)  then 
nearest_range  :=  _range(k); 
end  if; 

end  loop; 

if(frame_update  =  0  )  then 

if(hw_receive_test(host))  then 

hw_receive(host,  dummy,  lsw); 
dumptohost; 
end  if; 

else 

hidden_frame_cnt  :=  hidden_frame_cnt  +  1; 
if(hidden_frame_cnt>=frame_update)  then 
ll_receive(host,dummy4sw); 
dumptohost; 
hidden_frame_cnt  :=  0; 
end  if; 

end  if; 

end  dohostio; 
begin — main 

initialize Jiostio; 

loop 

dohostio; 

end  loop; 

end  hostio; 

9.3.2  Target  Generation  Code  threat.ada 

#ifndef  NET 
#define  NET  net 
#endif 

procedure  threat  is 


—  Take  the  center  of  Earth  as  x,y,z=(0,0,0)  for  earth  relative 

—  cordinates  fpa  coordinates  is  a  cartesian  coordinate 

—  system  with  the  orgin  on  the  interceptor.  The  negative  z  axis 

—  of  this  coordinate  system  extends  through  the  center  of 

—  the  fpa  out  into  the  field  of  view,  normal  to  the  fpa. 

—  The  x  axis  is  runs  through  the  extended  plane  of  the 

—  fpa  horizontally,  and  the  y  axis  vertically. 


GRAV  :  constant  float  :=  6.67E-1 1 ; 
EarthRadius  :  constant  float  :=  6.37E+06; 
EarthMass  :  constant  float  :=  5.97E+24; 
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Arrayiizc  :  constant  integer  :=  128; 

Z_OFFSET  ;  constant  float  r=  6.37E+06; 

RAD _PER_PXE L :  constant  float  :=  75.0E-06; 

PXEL_PER_RAD  :  constant  float  :=  13333.3333;  —  1.0  /  RAD.PER.PXEL 
Negarive_One :  constant  float  :=  -1.0; 

PI  :  constant  float  :=  3.14159265; 
done  ;  constant  integer  :=  -333; 

type  A_3x3  Jloat.array  is  array  (0..2, 0.  .2)  of  float; 
type  three.element.float.anay  is  array  (0..2)  of  float; 

aim  ;  A_3x3_float_array;  —  orientation  of 

—  interceptor  in  radians 

Missile_Coord  :  thrce_element _float_array;  —  X  of  KEW 

—  missile  World  coord 

Mis iile_Velocity  :  three_elemenl_float_array;  —  Vel  of  KEW  missile  World  coord 

mr  :  three.element_float.anay;  —  location  of  this  object  in 

—  missile  ref  frame 

distance :  float; 
previous.distance :  float; 

threat  .intense  :  float;  —  object  brightness 

Thrcat_Coord  :  three_element.float_array;  —  X  of  target 
Threat.Velodty ;  three.element.float.array;  —  velocity  of  target 
threat.size :  float;  —  diameter  of  the  target 

time_step  ;  float;  —  Time  step  to  take 

velocity :  float; 

t :  float;  —  time 

debugtemp :  integer, 

temp  :  float; 
temp.area :  float; 

—  intent  :  float; 

Thieat.Number :  integer, 

miss_di stance  :  float; 

pixels,  pixel_y,  radius,  total_in tensity  :  integer, 
intensity :  integer, 

delta_theta,delta_phi  :  float;  —  interceptor  rotation  in  radians 

deltas,  delta^y :  float; 

procedure  compute_KEW_position  is 

r*r3»gx^y,gz,  z :  float; 
ar :  anay(0..1)  of  float; 
am  :  three_element_float_array; 


begin 

z  r=  Missile.Coord(2)  +  Z.OFFSET; 

r  :=  sqit((  Missile_Coord(0)*Missile_Coord(0)  +  Missile_Coord(l)*Missile  Coord(l)  +  z*z) ); 
i3  := (GRAV  *  EarthMass) /  (r*r*r); 

gx  r=  -Mis sile_Coord (0)  *  r3; 
gy  -Missile_Coord(l)  *  r3; 
gz  :=  -z  *  r3; 

ll_receive(NET,  delu_x); 
ar<0)  :=  delta  jt  *  time.step; 

ll.ieceive(NET,  delta_y); 
ar(l)  s=  delta^y  *  time_step; 

am(0)  :=  aim(0,0)  *  ar<0)  +  aim(O.l)  *  ar(l); 
am(l)  :=  aim(l,0)  *  ar<0)  +  aim(l.l)  *  ar<l); 
am(2)  :=  aun(2,0)  *  ar<0)  +  aim(2,l)  *  ar(l); 
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Missilc_Velocity(0)  :=  Mis sile_ Velocity (0)  +  (am(0)+  gx)  *  time_step; 
Mis sile_ Velocity ( 1 )  :=  Missile_Velocity(l )  +  (am(l)+  gy)  *  time_step; 
Missile_Velocity(2)  :=  Mis sile_  Velocity (2)  +  (am(2)+  gz)  *  time_step; 

Missile_Coord(0)  :=  Missile_Coord(0)  +  Missile_Velocity(0)  *  time_step; 
Missile_Coord(l)  :=  Missile_Coord(l)  +  Missile_Velocity(l)  *  time_step; 
Mis sile_Coord(2)  :=  Missile_Coord(2)  +  Missile_Velocity(2)  *  time_step; 


—  r  =  axb 

procedure  cross_prod(r  :  out  three_element_float_array; 

a :  in  three_element_float_array; 
b  :  in  three_element_float_array)  is 

begin 

r(0)  :=  a(l)*b(2)  -  a(2)*b(l); 
i(l)  :=  a(2)*b(0)  -  a(0)*b(2); 
i(2)  :=  a(0)*b(l)  -  a(l)*b(0); 

end; 

—  Given  thetax,  thetay,  calculate  new  position  Orientation  matrix  A 
procedure  rotate(A  :  in  out  A_3x3_float_array; 

thetax :  in  float; 
thetay  :  in  float)  is 

p,q,r :  three_element_float_array; 
pmag,qmag,rmag  :  float; 


begin 


—  Calculate  rotation  about  x  axis 
for  i  in  0..2  loop 

p(i)  :=  A(it0)  +  A(i,2)*thetay; 
q(i)  :=  A(i,l)  +  A(if2)*thetax; 
end  loop; 

—  force  orthogonal  coordinate  system 

cross_prod(r,p,q);  —  r^=pxq 

cross_prod(q,r,p);  —  q=rxp 

—  Force  vectors  to  be  unit  length  ! 

pmag  :=  sqrt((  p(0)*p(0)  +  p(l)*p(l)  +  p(2)*p(2)) ); 
qmag  :=  sqrt((  q(0)*q(0)  +  q(l)*q(l)  +  q(2)*q(2)) ); 
rmag  :=  sqrt((  r(0)M0)  +  KDMD  +  r(2)*r(2))  ); 

for  i  in  0.. 2  loop 

A(i,0)  :=  p(i)/pmag;  —  pO  qO  iO 
A(i,l)  :=  q(i)/qmag;  —  A  =  pi  ql  rl 
A(i,2)  :=  i<i)/rmag;  —  p2  q2  r2 
end  loop; 


end; 


—  User  Cramers  rule  to  solve  t  assumes  det  of  matrix  =  1 
procedure  solve(A  :  in  A_3x3_float_array; 

x :  out  three_element_float_array; 
b  :  in  three_element_float_aijay)  is 
tmpl,tmp2,tmp3  :  float; 

begin 


tmpl  :=  b(0)  *  (  A(l,l)  *  A(2,2)  -  A(2,l)  *  A(l,2)); 
lmp2  :=  A(0.1)  *  (  b(l)  *  A(2,2)  -  A(l,2)  *  b(2)); 
tmP3  :=  A(0,2)  *  (  b(l)  *  A(2,l)  -  A(lf  1)  *  b(2»; 
x(0)  :=  tmpl  -  tmp2  +  tmp3; 


— *(0)  :=  ( 

—  b(0)  *  (  A(1,1)*A(2,2)  -  A(2,l)*A(lt2) ) 

—  -  A(0fl)  *  ( b(l)  *A(2,2)-A(lf2)*b(2)  ) 

—  +  A(0,2)  *  (  b(l)  * A(2, 1 )  -  A(1 ,1  )*b(2)  ) 

—  ); 
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tmpl  :=  A(OtO)  *  (  Ml)  *  A(2,2)  -  b(2)  *  A(U)); 
tmp2  .*=  b(0)  *  (  A(1,0)  *  A(Z2)  -  A(l,2)  *  A(2,0)); 
lmp3  :=  A(0,2)  *  (  A(1,0)  *  b(2)  -  b(l)  *  A(2,0)); 
x(l)  :=  tmpl  -  tmp2  +  tmp3; 


A(0,0)*  (b(l)  *A(2.2)-b(2)  *A(1,2)) 

-  MO)  *  (  A(1.0)*A(2,2)  -  A(1,2)*A(2,0) ) 
+  A(0,2)*(A(1,0)*  M2) -Ml)  *A(2,0) ) 
); 


toipl  r=  A(0,0)  *  (  A(l,l)  *  M2)  -  A(2,l)  *  Ml)); 
tmp2  :=  A(0,1)  *  (  A(1,0)  *  M2)  -  Ml)  *  A(2,0)); 
tmp3  MO)  *  (  A(1,0)  *  A(2,l) -  A(U)  *  A(2,0)); 
x(2)  :=  tmpl  -  tmp2  +  tmp3; 


A(0,0)*(A(ltl)*M2)  -A(2»1)*M1)) 

“ A(0,1) * ( A(1,0)*M2)  -Ml)  *A(2,0) ) 
+  MO)  *(A(lt0)*A(2,l)-A(U)*A(210)) 
); 


end; 


—  compute  current  position  and  velocity 

procedure  ballisdc(x  :  in  out  thiee_element_float_array  ; 

v  :  in  out  three_element_float_array; 
mow :  in  out  float)  is 

r,r3.gx,gy,gztz :  float; 


z  :=  x(2)  +  Z  OFFSET; 
r  :=  sqrt(  (  x(0)*x(0)  +  x(l)*x(l)  +  z*z  )); 
r3  r=  (GRAY  *  EarthMass)  /(r*r*r); 
gx  :=  -x(0)  *  r3; 
gy  :=  -x(l)  ♦  r3; 
gz  s=  -z  *  r3; 

v(0)  :=  v(0)  +  gx  *  time_step; 
v(l)  ss  v(l)  +  gy  *  time_step; 
v(2)  :=  v(2)  +  gz  *  time_step; 

x(0)  :=  x(0)  +  v(0)  *  time_step; 
x(l)  :=  x(l)  +  v(l)  *  ume^step; 
x(2)  •=  x(2)  +  v(2)  *  time__step; 


end; 


—  initialize  these  things 

— initialize_th rcat_object (aim ,  threaten  tense,  Threat_Coord,  Threat_Velocity,  threal_size,  time_step,  t ); 

procedure  inidalize_threat_object  is 
temp  rfloat; 


begin 

miss_distance  :=-1.0; 

—  Receive  missile  aim  AIM 

for  i  in  0.2  loop 

for  j  in  0..2  loop 

U_receive(host,  temp); 
aim(ij)  :=  temp; 

—  ll_send(host,aim(i  j)); 
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end  loop; 
end  loop; 

—  Receive  missile  coordinates  XM 

for  i  in  0..2  loop 

ll_receive(host,  temp); 
Missile_Coord(i)  :=  temp; 

—  ll_send(host,Missile_Coord(i)); 
end  loop; 

—  Receive  missile  velocity  VM 

for  i  in  0..2  loop 

ll_rcceive(host,  temp); 
Missile_Velodty(i)  :=  temp; 

—  ll_send  (host,Mi  s  sile_Velodty  (i)); 
end  loop; 

ll^rcceive^ost,  timc_step); 

—  ll_send(host,  time_step); 

Threat_Number  :=  0; 
ll_receive(host,  Threat_JNumber,  lsw); 

—  ll_send(host,  Threat_Number,  lsw); 

—  Receive  threat  coordinates  XO 

for  i  in0..2  loop 

ll_receive(host,  temp); 
Threat_Coord(i)  :=  temp; 

—  U_send(host,Threat_Coord(i)); 
end  loop; 

—  Receive  threat  velocity  VO 

for  i  in  0..2  loop 

ll_receive(host,  temp); 
Threat_Velocity(i)  :=  temp; 

—  U_send(host,Threat_Velocity(i)); 
end  loop; 

U_receive(host,  threat_si2e); 

—  ll_send(host,  threat_size); 

ll_receive(host,  threat_intense); 

—  ll_s  end  (host,  threat_intense); 

t  :=  0.0; 

pre vious_di stance  :=  2.0e+30; 
distance  :=  1.0e+30; 


procedure  fpa_map  is 
ftl,  ft2 :  float; 

—  Calculate  which  pixels  are  actually  turned  on 

—  see  note  at  top  for  weird  stuff  in  y  on  missile 

—  relative  coordinate  system 

begin 


radius  :=  integer(atan(threat_sizeAnr(2))  *  PXEL_PER_RAD); 
if  (radius  <  0)  then 

radius  :=  (-radius); 
end  if; 

if  (mr(2)  <  0.0)  then 

ftl  :=  -0.5  +  (PXEL_PER_RAD  *  atan(  mr(0)/(-mr(2))  )); 
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pixel jt  :=  ArraySize/2  +  integer(  ftl ); 

ft2:=-0.5  +  (PXEL_PER_RAD  *  atan(  mr(l)/(-mr(2)) )); 

pixel  _y  :=  AnaySize/2  +  integer<  fi2 ); 

end  if; 

if  ((mr(2)  >  0.0)  or  (pixel_x  >  (Array  Size-1)) 

or  (pixel _jt  <  0)  or  (pixeLy  >  (ArraySize  -  1))  or  (pixel_y  <  0))  then 
pixel_x  :=-l; 
pixel_y  :=  -1; 
radius  r=  0; 

end  if; 


—  Here  is  where  we  figure  out  how  big  it  is,  and  how  bright. 

—  Intensity  goes  as  inverse  r  squared 

total Jn tensity  :=  integer(  threat_intense  /  (mr(2)  *  mr(2))); 
if  (radius  >  l)then 

total_in tensity  ;=  toial_in tensity  /  (radius  *  radius); 
end  if; 

if  (total_in  tensity  <  2 )  then 
toul_in tensity  :=  1; 
end  if; 

if  (total_in tensity  >  63  )  then 
total_in tensity  :=  63; 
end  if; 


end; 


—  finds  the  relative  vector  from  xb  to  xa,  then  transforms 

—  by  the  inverse  of  the  rotation  matrix  rot. 

—  Result  is  put  in  res,  note  that  all  of  these 

—  must  be  declared  1 

procedure  rel_vector(xa  :  in  three_element_float_array; 

xb :  in  three_element_float_anay; 

rot:  in  A_3x3_float_array; 

res:  out  thrce_element_float_array)  is 

d :  three_element_fioat_array; 

begin 

d(0):=(xa(0)-xb(0)); 
d(l):=(xa(l)-xb(l)); 
d(2)  .*=  (xa(2)-xb(2)); 

sdve(rot^es,d); 

end; 


procedure  transpose(A  :  in  A_3x3_float_array; 

B  :  out  A_3x3_float_array)  is 


begin 

for  i  in  0..2  loop 

for  j  in  0..2  loop 
B(j4)  -  A(ij); 
end  loop; 
end  loop; 

end; 

function  compute_miss_distance  return  float  is 
t,dt,dist^>rcv_dist :  float; 

dx,tm_Missile_Coord,tm_Thrcat_Coord  :  three_element_float_array; 


begin 
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dt:=  0.00001; 
dist:=1.0e30; 
prcv_dist  :=  2.0e30; 
for  i  in  0..2  loop 

tm_Missile_Coord(i)  :=  Missile_Coord(i); 
tm_Threat_Coord(i)  :=  Thrcat_Coord(i); 
end  loop; 

while(prev_dist  >  dist )  loop 
prev_dist  :=  dist; 
for  i  in  0..2  loop 

tm_Missile_Coord(i)  :=  tm_Missile_Coord(i)  -  Missile_Velocity(i)  *  dt; 
tm_Threat_Coord(i )  :=  tm_Threat_Coord (i)  -  Threat_Velocity(i)  *  dt; 
end  loop; 

for  i  in  0.. 2  loop 

dx(i)  :=  tm  Mis sile_Coord(i)  -  tm _Threat_Coord(i); 
dx(i):=dx(7)*dx(i); 
end  loop; 

dist  :=  sqrt(  dx(0)  +  dx(l)  +  dx(2)  ); 
end  loop; 
return  (prev_dist); 


—  Main —  main  — 
begin 


pixel_x  :=  0; 
pixel_y  :=  0; 
radius  :=  O, 
intensity  :=0; 
distance  :=  0.0; 

Missile__Coord(0)  :=  0.0; 

Missile_Coord(l)  :=0.0; 

Missile_Coord(2)  ;=  0.0; 

Threat_Coord(0)  :=  0.0; 

Threat_Coord(l)  :=  00.0; 

Th reat_Coord (2)  :=  00.0; 
miss_distance  :=  -1.0; 

initialize_threat_object; 

loop 

U_send(NET,  pixel_x,lsw); 

—  ll_send(host,  pixel_x,lsw); 

ll_send(NET,  pixel _yjsw); 

—  ll_send(host,  pixel_y,lsw); 

ll_send(NET,  radius  4s  w); 

—  ll_send(host,  radiustlsw); 

ll_send(NET,  intensity); 

—  ll_send(host,  intensity); 

ll_send(NET,  distance); 

—  ll_send(host,  distance); 

if  Threat_Number  =  0  then 
for  i  in  0..2  loop 

temp  :=  Missile_Coord(i); 
ll_send(NET,  temp); 

—  ll_send(host,  temp); 
end  loop; 
end  if; 

for  i  in  0..2  loop 

temp  :=  Threat_Coord(i); 
ll_send(NET,  temp); 

—  ll_send(host,  temp); 
end  loop; 

ll_send(NET,  miss_distance); 

—  ll_send(host,  miss_distance); 
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ll_receive(NET,  delta  jheta);  —  delta-theta 

ll_reccivc(NET,  delu_phi);  —  delta-phi 

.  t  :=  t  +  time_step; 

—  compute  ballistic  path  of  this  object 
ballistic(Th  reat_Coord,Threat_\fclodty,t); 

compute_KEW_position; 

roUte(iim,delta_theta, delta __phi);  —  Update  Missile  orientation 

id_vector(Thieat_CoordtMissile_Coord,aim^nr); 

fpa_map; 


temp_area  :=  PI  *  float(radius*radius); 
if  (temp_area  <  1.0)  then 
temp__area  :=  1.0; 
end  if; 

intensity  :=  total_intensity  *  integer(temp_area); 

distance  :=  sqit((mr(0)*mr(0)  +  mr(l)*mr(l)  +  mr(2)*mr(2))); 

if  (distance  >  previous_distance)  and  (mis s_di stance  <  0.0  )  then 
miss.distance  r=  oompute_miss_distance; 
end  if; 

prcvious_distance  :=  distance; 

end  loop; 
end; 


9.3 3  Target  Selection  track.ada 

#ifndef  NET 
#defme  NET  net 
#endif 

#define  BOGUS  bogus  :=  1.0 
procedure  TRACK  is 


—  Procedure  object_traclring 

—  Receives  cluster  data  from  clustering  chip. 

—  Predicts  the  movement  of  each  cluster  in  tfEstimateVx, 

—  and  tfEstimateVy.  Receives  updated  orientation  of  the 

—  FPA  in  delta_theta  and  delta_phi 


ArraySize  :  constant  integer  :=  128; 


RadiansPerPixd  :  constant  float  :=75.0E-6; 
MaxTracks  :  constant  integer  :=  32; 
MaxTracksPl  ;  constant  integer  r=  33; 
MaxMiss  :  constant  integer  :=  4; 
IntensityWeight  :  constant  float  :=  0.5; 
AreaWeight  :  constant  float  :=  0.4; 
NumCorWeight  :  constant  float  ;=  0.1; 
Gravity  :  constant  float  :=  6.67E-1 1; 
Earth_Radius  :  constant  float  :=  6.36E06; 
Earth_Mass  :  constant  float  :=  5.97E24; 
FPA_WIN_X  :  constant  integer  :=  30; 
FPA_WIN_Y  :  constant  integer  :=  50; 
PAN_WTN_X  :  constant  integer  :=  20; 
PAN_WIN_Y  :  constant  integer  :=  350; 


done 


:  constant  integer  :=  -555; 


tcTrackNum  ;  array  (0..33)  of  integer; 
tcPriority  :  array  (0..33)  of  integer; 
tcRPriority  :  array  (0..33)  of  float; 
tcPositionX  :  array  (0..33)  of  float; 
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tcPositionY  :  array  (0..33)  of  float; 
tcWeightX  :  array  (0..33)  of  float; 
tcWeightY  ;  array  (0..33)  of  float; 
tcMinDistance  :  array  (0..33)  of  float; 
tcNumCor  ;  array  (0..33)  of  float; 
tcCenID  :  array  (0..33)  of  float; 

tcCcnAcoorX  :  array  (0..33)  of  float; 
tcCenAcoorY  :  array  (0..33)  of  float; 
tcCenlcoorX  :  array  (0..33)  of  float; 
tcCenlcoorY  :  array  (0..33)  of  float; 
tcRange  :  array  (0..33)  of  float; 
tcCenArea  :  array  (0..33)  of  integer; 
tcCenlntensity  :  array  (0..33)  of  integer, 

tfTrackID  :  array  (0..32)  of  integer; 
tfNumCor  :  array  (0..32)  of  integer, 
tfNumMiss  :  array  (0..32)  of  integer, 

tfEstimateX  :  array  (0..32)  of  float; 
tfEstimateVX  :  array  (0..32)  of  float; 
tfXPl  1  :  array  (0..32)  of  float; 

tfXP12  :  array  (0..32)  of  float; 

tfXP22  :  array  (0..32)  of  float; 

tfArea  :  array  (0..32)  of  float; 
tfEstimateY  :  array  (0..32)  of  float; 
tfEstimateVY  :  array  (0..32)  of  float; 
tfYPl  1  :  array  (0..32)  of  float; 

tfYP12  :  array  (0..32)  of  float; 
tfYP22  :  array  (0..32)  of  float; 
tfCenAcoorX  :  array  (0..32)  of  float; 
tfCenAcoorY  :  array  (0..32)  of  float; 
tfCenlcoorX  :  array  (0..32)  of  float; 
tfCenlcoorY  :  array  (0..32)  of  float; 
tfRange  :  array  (0..32)  of  float; 
tflntensity  :  array  (0..32)  of  integer, 

tfCSO  :  array  (0..32)  of  integer, 

cID  :  integer; 
cAcoorX  :  float; 
cAcoorY  :  float; 
clcoorX  :  float; 
cIcoorY  ;  float; 
cArea  :  integer, 
clntensity :  integer, 
cRange  :  float; 

pixel_x,  pixel_y  :  integer, 

LastTrack,  LastNew  :  integer, 

TraddD,  CentroidID :  integer; 

ptr,  lowptr  :  integer; 

dx,  dy,  dist2,  priority,  lowpriority  :  float; 

NotMatchCentroid  :  boolean; 

LastCentroid  :  integer; 

temp :  float; 
debugtemp :  integer, 
bogus  :  float; 

iterations  :  integer, 
dt,  dt2,  dtsqr  :  float; 

RadPix2,  Pixels PerR ad ian,  ProcessNoise,  MeasurementNoise  :  float; 
initialPl  1 ,  initialP12,  initialP22,  maxNumCor  :  float; 

PixelOffset :  float; 

x_dot,  y_dot :  float; 

Targetlntensity :  float; 
mge :  float; 

MaxDist,  time :  float; 
previous_x,  previous_y :  float; 

Num_obj  :  integer, 

FinalAcquisition :  boolean; 

TrackingRange :  float; 
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PixelTrackingRange :  integer, 

RnalAcquisitionRange :  float; 

TargetSelectTime ,  delta_theta,  delu_phi,  theta_dot,  phi_dot :  float; 

TaigetTrack :  integer, 

TargetSelected  :  boolean; 

frame  :  integer, 
error  :  boolean; 

—  Variables  local  to  main  routine. 
xdot_range_comp,  y_dot_nmge_comp,  a  :  float; 


procedure  KalmanTimeUpdate  (position  :  in  out  float; 

velocity :  in  out  float; 
Pll  :  in  out  float; 

P12  :  in  out  float; 

P22  :  in  out  float)  is 


begin 

position  :=  position  +  (dt  *  velocity); 

PI  1  :=  PI  1  +  ((dt2  *  P12)  +  (dtsqr  *  P22)); 
P12  :=  P12  +  (dt  *  P22); 

P22  :=  P22  +  ProcessNoise; 
end  KalmanTimeUpdate; 


procedure  KabnanMeasurementUpdate  (MeasuredPosition  :  in  float; 
position  :  in  out  float; 

velocity :  in  out  float; 

Pll  :  in  out  float; 

P12  :  in  out  float; 

P22  :  in  out  float)  is 


temp,  Kl,  K2,  StateError :  float; 
begin 

StateError  r=  MeasuredPosition  -  position; 
temp  :=  1.0  /  (Pll  +  MeasurementNoise); 
Kl  :=  temp*  Pll; 

K2  :=  temp  *  PI  2; 
temp  :=  P12; 

Pll  r=(1.0-Ki)  *  Pll; 

P12  :=(1.0-K1)  *  P12; 

P22  :=  P22  -  (K2  *  temp); 
position  :=  position  +  (Kl  *  StateError); 
velocity  :=  velocity  +  (K2  *  StateError); 
end; 


procedure  Initialize_track_object  is 
begin 


ll_receive(host,dt); 
dt2  :=  2.0  *  dt; 
dtsqr  dt  *  dt; 

FmalAcquisition  :=  false; 

RadPix2  :=  RadiansPerPixel  *  RadiansPerPixel; 
PixelsPerRadian  :=  1.0 /RadiansPerPixel; 
ProcessNoise  :=  2.5  *  RadPix2; 
MeasurementNoise  :=  1.0  *  RadPix2; 

TaigetTrack  :=  0; 
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Initial  PI  1  :=  2.0*RadPix2; 

InitialP12  :=  0.0  *  RadPix2; 

InitialP22  :=  100.0  *  RadPix2; 

PixelOfifsct  :=  float(ArraySize)  /  2.0  -  0.5; 

LastTrack  :=  0; 

LastNew  t=  0; 
for  i  in  O..MaxTracks  loop 
tcTrackNum(i)  :=  i; 
tcPriority(i)  :=  0; 
end  loop; 

TrackID  :=0; 

TargetSelected  :=  false; 
lime  :=  0.0; 
x_dot  :=  0.0; 
y_dot  :=  0.0; 
theta_dot  :=  0.0; 
phi_dot  :=  0.0; 

frame  :=  0; 
error  r=  false; 

U_receive(hosttTaigetSelectTime); 

Num_obj  :=  0; 

ll_receive(host,Num_obj  ,lsw); 
ll_rcceive(host,HnalAcquisitionRange); 

PixelTrackingRange  :=  0; 
U_receive(hosttPixelTrackingRangetlsw); 

TrackingRange  :=  float  (PixelTrackingRange)  *  RadiansPerPixel; 
end  Initialize_track_object; 


procedure  Compensatc_FPA_Rotation  is 
begin 

for  i  in  0.. LastTrack- 1  loop 

tfEstimateX(tcTrackNum(i))  :=  tfEstimateX(tcTrackNum(i))  -  delta_phi; 
tfEstimateY(tcTrackNum(i))  :=  tfEstimateY(tcTrackNum(i))  -  delta_theta; 
end  loop; 
end; 


procedure  UpdateTracks  is 
begin 

for  i  in  0..(LastTrack-l)  loop 

—  check  for  multiple  centroid  associations  — 

IF  (tcNumCortf)  >  0.0)  AND  (tcCenlD(i)  >=  0.0)  THEN 
ptr:=i+  1; 

WHILE  (ptr  <  LastTrack)  loop 
IF  (tcCenlD(i)  =  tcCenID(ptr))  THEN 
IF  (tcMinDistance(i)  >  tcMinDistance(ptr))  THEN 
tcCenID(i)  :=  -1.0; 

ELSE 

tcCenID(ptr)  :=  -1.0; 

END  IF; 
end  if; 

ptr  :=  ptr  +  1; 


end  loop; 
end  if; 

IF  (tcNumCor(i)  =  0.0)  THEN 
—  no  correlation  with  track  file  — 

IF  tfNumMis s (tcTrackN um (i))  >=  MaxMiss  THEN 
tcPriority(i)  :=  0; 

ELSE 

tfNumMiss(tcTrackNum(i))  :=  tfNumMiss(tcTrackNum(i))  +  1; 
tcPriority(i)  :=  tfNumCor(tcTrackNum(i))  -  tfNumMiss(tcTrackNum(i)); 
end  if; 

ELSE 

IF  (tcCenID(i)  =  (-1.0))  THEN 
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—  correlation  but  another  track  file  is  closer  — 


tfNumMiss(tcTrackNum(i))  :=  0; 
tcPriority(i)  :=  tfNumCor(tcTracicNum(i)); 
KalmanMea*urementUpdate(tcPositionX(i), 
tfEstjmateX(tcTrackNum(i))( 
tfEstimateVX(tcTrackNum(i)), 
tfXP  1 1  (tcTrackNum  (i)), 
tfXPl  2(tcTrackNum(i)), 
tfXP22(tcTrackNum(i))); 
KalmanMeasurementUpdate(tc  Position  Y(i), 
tfEstimate  Y  (tcTrackNum(i)), 
tfEstimate  VY(tcTrackNum(i)), 
tfYPl  1  (tcTrackNum(i)), 
tfYP12(tcTrackNum(i)\ 
tfYP22(tcTrackNum(i))); 


ELSE 

—  update  from  correlated  centroid  — 

tfNumCor(tcTrackNum(i))  :=  tfNumCor(tcTrackNum(i))  +  1; 
t£NumMiss(tcTrackNum(i))  :=  0; 
tcPriorityG)  :=  tfNumCor(tcTrackNum(i)); 
tflnten  sity  (tcTrackNum  (i))  :=  tcCenlntensity(i); 
tfArea(tcTraclcNum(i))  :=  float(tcCenArea(i)); 
tfRange(tcTrackNum(i))  :=  tcRange(i); 
tfCenAcoorX(tcTrackNum(i))  :=  tcCenAcoorX(i); 
tfCenAcoorY(tcTrackNum(i))  :=  tcCenAcoorY(i); 
tfCenIcoorX(tcTrackNum(i))  :=  tcCenIcoorX(i); 
tfCenIcoorY(tcTrackNum(i))  :=  tcCcnlcoorY(i); 

KalmanMeasurementUpdate(tcCenAcoorX(i)> 
tfEstimateX(tcTrackNum(i))f 
tfEstimate VX(tcTiackNum(i)), 
tfXPl  l(tcTrackNum(i)), 
tfXP12(tcTrackNum(i)), 
tfXP22(tcTrackNum(i))); 

KalmanMeasu  rementUpdate<tcCenAcoor  Y  (i), 
tfEstimate  Y  (tcTrackNum(i)), 
tfEstimate  VY(tcTrackNum(i)), 
tfYPl  1  (tcTrackNum  (i)), 
tfYP12(tcTrackNum(i)), 
tfYP22(tcTrackNum(i»); 

end  if; 
end  if; 
end  loop; 
end; 


procedure  InitializeNewTracks  is 
begin 

fori  in  LastTrack..(LastNew  - 1)  loop 
tfTrackID(tcTrackNum(i))  :=  TrackID; 

TrackID  :=  TrackID  +  1; 
tfNumCor(tcTrackNum(i))  :=  1; 
tfNumMiss(tcTrackNum(i))  :=  0; 
tcPriority(i)  :=  1; 

tfIntensity(tcTrackNum(i))  :=  tcCenlntensity(i); 
tf  Area  (tcTrackNum  (i))  :=  float(tcCenArea(i)); 

tfRange(tcTrackNum(i))  :=  tcRange(i); 
tfEstimateX(tcTrackNum(i))  :=  tcCenAcoorX(i); 
tfEstimate  VX(tcTrackNum(i))  :=  0.0; 
tfXPl  l(tcTrackNum(i))  :=  initialPll; 
tfXPl  2(tcTrackNum(i))  :=  initialP12; 
tfXP22(tcTrackNum(i))  :=  inidalP22; 
tfEstimateY(tcTrackNum(i))  r=  tcCenAcoorY(i); 
tfEstimate  VY(tcTrackNum(i))  :=  0.0; 
tfYPl l(tcTrackNum(i))  :=  initialPll; 
tfYP  12(tcTrackNum(i»  :=  initialP12; 
tfYP22(tcTrackNum(i))  :=  initialP22; 
tfCenAcoorX(tcTrackNum(i))  :=  tcCenAcoorX(i); 
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tfCenAcoorY(tcTrackNum(i))  :=  tcCenAcoorY(i); 
tfCenIcoorX(tcTrackNum(i))  :=  tcCenIcoorX(i); 
tfCenIcoorY(tcTrackNum(i))  :=tcCenIcoorY(i); 

end  loop; 
end; 


procedure  RankTracks  is 
tempi,  temp2 :  integer, 
begin 

IF  (LastNew  >  1)  THEN 
—  sort  on  priority  — 
fori  in  O..LastNew-2  loop 

ptr:=i; 

forj  in  (i+l)..(LastNew-l)  loop 
IF  (tcPriority(i)  <  tcPriority(j))  THEN 
ptr:=j; 
end  if; 
end  loop; 

IF  (ptr  /=  i)  THEN 

tempi  :=  tcTrackNum(ptr); 
temp2  :=  tcPriority(ptr); 
tcTrackNum(ptr)  :=  tcTrackNum(i); 
tcPriority(ptr)  :=  tcPriority(i); 
tcTrackNum(i)  :=  tempi; 
tcPriority(i)  :=  temp2; 
end  if; 
end  loop; 

end  if; 

LastTrack  :=  0; 

WHILE  (tcPriority(LastTrack)  >  0)  loop 
LastTrack  :=  LastTrack  +  1; 
tempi  :=  tcPriority(LastTrack); 
end  loop; 

LastNew  :=  LastTrack; 
end; 


procedure  SetUpCorrelation  is 
begin 

for  i  in  0..(LastTrack  - 1)  loop 

KalmanTimeUpdate(  tfEstimateX(tcTrackNum(i)), 
tfEstirnateVX(tcTrackNum(i)), 
tfXPl  1  (tcTrackNum(i)), 
tfXPl  2(tcTrackNum(i)), 
tfXP22(tcTrackNum(i))); 

KalmanTimeUpdate(  tfEstimateY(tcTrackNum(i)), 
tfEstimate  V  Y  (tcTrackNum(i)), 
tfYPl  1  (tcTracknum(i)), 
tfYP12(tcTrackNum(i)), 
tfYP22(tcTrackNum(i))); 

tcPositionX(i)  :=  tfEstimateX(tcTrackNum(i)); 
tcPositionY(i)  :=  tfEstimateY(tcTrackNum(i)); 
tcWeightX(i)  :=  1.0  /  tfXPl  l(tcTrackNum(i)); 
tcWeightY(i)  :=  1.0  /tfYPl  l(tcTrackNum(i)); 
lcMinDistance(i)  :=  MaxDist; 
tcNumCor(i)  :=  0.0; 
end  loop; 
end; 
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procedure  InputCentroid  is 

iAcoorX,  iAcoorY,  ilcoorX,  ilcoorY  :  float; 
temp :  float; 

begin 

pixel jt  :=  0; 
pixel_y  r=  0; 

U_receive(NET,pixel_x,lsw); 
Ujrccrive(NET,pixel._yflsw); 
iAcoorX  :=  float(pixeLx); 
iAcoorY  :=  float(pixel_y); 
ilcoorX  :=  iAcoorX; 
ilcoorY  :=  iAcoorY; 

cAiea  :=  0; 

Il_receive(NHT,  cArea  Jsw); 
fl_receive(NET,cIn  tensity); 
n_recrive(NET,cRinge); 

cID  :=  CentroidID; 

CentroidID  :=  CentroidID  +  1; 

cAcoorX  :=  (iAcoorX  -  PixelOffset)  *  Radians PerPixel; 
cAcoorY  :=  (iAcoorY  -  PixelOffset)  *  RadiansPerPixel; 
clcoorX  :=  (ilcoorX  -  PixelOffset)  *  RadiansPerPixel; 
cIcoorY  (ilcoorY -PixelOffset)  *  RadiansPerPixel; 

f*  debugtemp  :=  1; 

U_send(host,debugtemp4*w);  */ 

end; 


procedure  PerfotmTracking  is 
centroid_ctr :  integer, 

begin 

centroid.ctr  :=  O, 

Compensate_FPA_Rotation; 

SetUpCorrclation; 

LastCentroid  :=  O, 

CentroidID  :=  O, 

WHILE  (centroid_ctr  <  Num_obj)  loop 
centroidjctr  :=  centroid_ctr  +  1; 

InputCentroid; 

—  if  cArea  >  0  then 

—  compare  against  tracks  — 

NotMatchCentroid  :=  TRUE; 
ptr  :=0, 

WHILE  (ptr  <  LastTrack)  loop 

dx  :=  cAcoorX  -  tcPositionX(ptr); 

dy  :=  cAcoorY  -  tcPosition  Y(ptr); 

dist2  :=  tcWeightX(ptr)*(dx*dx)  +  tcWeightY(ptr)  *  (dy*dy); 

IF  dist2  <  MaxDist  THEN 

—  correlated  with  this  track  file  — 

tcNumCor(ptr)  :=  tcNumCor(ptr)  +  1.0; 
NotMatchCentroid  :=  FALSE; 

IF  dist2  <  tcMinDistance(ptr)  THEN 

tcMinDi  stance  (ptr)  :=  dist2; 
tcCenID(ptr)  :=  float(cID); 
tcCenAcoorX(ptr)  :=  cAcoorX; 
tcCenAcoorY(ptr)  ;=  cAcoorY; 
tcCenIcoorX(ptr)  ;=  clcoorX; 
tcCenIcoorY(ptr)  :=  cIcoorY; 
tcCenArea(ptr)  :=  cArea; 
lcRange(ptr)  :=  cRange; 

tcCenlntensity(ptr)  :=  clntensity; 
end  if; 
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end  if; 

ptr  r=pir  +  1; 
end  loop; 

—  compare  against  uncorrelated  centroids  — 

IF  (NotMatchCentroid  AND  (ptr  <  MaxTracks))  THEN 
—  check  to  start  new  track  — 

priority  :=  cAcoorX*cAcoorX  +  cAcoorY*cAcoorY; 

IF  LastNew  <  MaxTracks  THEN 

—  room  to  start  new  track  — 

tcRPriority  (LastNew )  :=  priority; 

tcCenID(LastNew)  :=  float(cID); 
tcCenAcoorX(LastNew)  :=  cAcoorX; 
tcCenAcoorY(LastNew)  :=  cAcoorY; 
tcCenlcoorX  (LastNew)  :=  clcoorX; 
tcCenIcoorY(LastNew)  :=  cIcoorY; 
tcCenArea(LastNew)  :=  cArea; 

tcRange(LastNew)  :=  cRange; 

tcCenlntensity  (LastNew)  :=  cln tensity; 

LastNew  :=  LastNew  +  1; 

ELSE 

—  check  other  new  track  for  priority  — 

lowpriority  :=  -1.0, 
lowptr  :=  LastNew; 

WHILE  (ptr  <  MaxTracks)  loop 

IF  (lowpriority  <  float(tcPriority(ptr)))  THEN 
lowptr  :=  ptr, 

lowpriority  :=  float(tcPriority(ptr)); 
end  if; 

ptr  :=  ptr  +  1; 
end  loop; 

IF  (priority  <  lowpriority)  THEN 

tcRPriority(jowptr)  :=  priority, 
tcCenID(lowptr)  :=  float(cID); 
tcCenAcoorXflowptr)  :=  cAcoorX; 
tcCenAcoorY  (lowptr)  :=  cAcoorY; 
tcCenIcoorX(lowptr)  :=  clcoorX; 
tcCenlcoorYflowptr)  :=  cIcoorY; 

—  tcCenArea(lowptr)  :=  cArea; 

—  tcRange(lowptr)  :=  cRange; 

—  Tan  had  an  error  in  his  code  it  read  as  follows  : 

tcCenArea(lowptr)  :=  integer(cRange); 

—  He  didnt  change  the  variable  name 

tcCenlntensity  (lowptr)  :=  clntensity; 
end  if; 

end  if; 
end  if; 

—  end  if; 
end  loop; 

UpdateTracks; 

InitializeNewTracks; 

RankTracks; 


procedure  SelectTarget  is 

maxArea,  maxNumCor,  Weight,  TaigetWeight :  float; 
maxlntensity :  integer, 

begin 

maxNumCor  :=  0.0; 
maxArea  :=  0.0, 
maxlntensity  :=  O, 

Taigetlntensity  :=  0.0; 

- —  Select  the  brightest  object  among  objects  that  meets  minimum 
—  number  of  Correlation. 
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for  i  in  O..La*tTrack  -  1  loop 

if  tfNumCor(i)  >  integer(maxNumCor)  then 
maxNumCor  :=  float(tfNumCor(i)); 
end  if; 

if  tfArea(i)  >  maxArca  then 
maxArca  :=  tfArea(i); 
end  if; 

if  tfln tensity (i)  >  maxln tensity  then 
maxlntensity  :=  tflntensity(i); 
end  if; 
end  loop; 

TaigetWeight  :=  0.0; 

TaigetTrack  :=-l; 

for  i  in  0..  LastTrack  -  1  loop 

Weight  :=  (NumCorWeight  *  float(tfNumCor(i))/maxNumCor)  + 
(AreaWeight  *  if  Area  (i)/max  Area)  + 

(Intensity Weight  *  float(tfIntensity(i))/ 
float(maxIntensity)); 
if  Weight  >  TaigetWeight  then 
TaigetTrack  :=  i; 

TaigetWeight  :=  Weight; 
end  if; 
end  loop; 

if  (LastTrack  >  0)  then 

TaigetSelected  :=  true; 
end  if; 

BOGUS;  BOGUS;  BOGUS; 

—  TaigetSelected  true; 

—  TaigetTrack  :=  0; 
end; 


procedure  BndTarget  is 
begin 

Taigetln tensity  :=  0.0; 

—  Select  the  brightest  object  among  objects  with  zero  misses 
for  i  in  O..LastTrack  -  1  loop 

if  (tfNumMiss(i)  =  0)  and  (float(tflntensity(i))  >  Taigetlntensity) 
then 

Targetln tensity  :=  float(tfIntensity(i)); 

TaigetTrack  :=  i; 
end  if; 
end  loop; 

end; 


—  Main  — 
begin 


Initi  alize_track_object; 


x_dot  :=  0.0; 
y_dot  :=  0.0; 
phi_dot  :=  0.0; 
theta_dot  :=  0.0; 


loop 

ll_send(NET»x_dot); 

ll_send(NET,y_dot); 

ll_send(NET,phi_dot); 

U_send(NET,theta_dot); 

debugtemp  r=  2; 

/*  U_send(host,debugtemp4sw);  */ 
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U_rcceive(NET,delta_theta); 
ll_rcccive(NETldelta_phi); 
mge  :=  tfRange(TargetTrack); 

if  mge  >  80.0E03  then 
MaxDist  :=  6.0; 

else 

'  MaxDist  :=  (80.0E03/mge)  *  6.0; 
end  if; 

PerformTracking; 
frame  :=  frame  +  1; 
time  :=  time  +  dt; 

if  (time  >=  TaigetSelectTime)  then 
if  (not  TaigetSelected)  then 
SelectTarget; 

phi_dot  :=  tfEstimateX(TargetTrack)  -  0.5  * 
RadiansperPixel; 

theta_dot  :=  tfEstimateY(TaigetTrack)  - 
0.5  *  RadiansPerPixel; 

xjdot  :=  mge  *  tan(tfEstimateVX(TargetTrack))/dt; 
y_dot  :=  mge  *  tan(tfEstimateVY(TaigetTrack))/dt; 


else 

if  (mge  <  FinalAcquisitionRange)  and 
((tfNumMiss(TaigetTrack)  >  0)  or 
(FinalAcquisition))  then 


FinalAcquisition  :=  TRUE; 

FindTaxget; 

mge  :=  tfRange(TaigetTrack); 

x_dot  :=  mge  *  tan(tfEstimateX(TargetTrack) 

-  0.5  *  RadiansperPixel)  /  dt; 

y_dot  :=  mge  *  tan(tfEstimateY(TargetTrack) 

-  0.5  *  RadiansperPixel)  /  dt; 
phi_dot  :=  tfEstimateX(TargetTrack)  - 

0.5  *  RadiansperPixel; 
theta_dot  :=  tfEstimateY(TaigetTrack)  - 
0.5  *  RadiansperPixel; 


if  tfNumMiss(TaigetTrack)  >  0  then 
FindTarget; 

mge  :=  tfRange(TaigetTrack); 
end  if; 

if  (tfEstimateX(TargetTrack)  >  TrackingRange) 

or  (tfEstimateX  (TargetTrack)  <  -TrackingRange) 

then 


phi_dot  :=  tfEstimateX(TargetTrack)  - 
0.5  *  RadiansperPixel; 

else 

phi_dot  :=  0.0; 
end  if; 

if  (tfEstimateY(TargetTrack)  >  TrackingRange) 
or  (tfEstimateY(TaigetTrack)  < 
-TrackingRange)  then 

theta_dot  :=  tfEstimateY(TargetTrack) 

-  0.5  *  RadiansperPixel; 

else 

theta_dot  :=  0.0; 
end  if; 

x  dot  :=  mge  *  tan(tfEstimateVX(TargetTrack)) 
/dt; 
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y_dot  :=  mgc  *  tan(tfEstimatcV  Y (TargetTrack)) 

/at; 

end  if; 

previous_x  :=  ifEstimalcX (TargetTrack); 
previously  :=  lfEstimateY(TargctTrack); 
end  if; 
end  if; 


end  loop; 
end; 


9.3.4  Guidance  los_gnc.ada 

#ifhdef  NET 
#define  NET  net 
#endif 

procedure  los  _gnc  is 

angNavConst,  linNavConst,  thrusterjnterval, 
time_stepf  ihraster_time,  max_divert :  float; 
divert:  float; 

—  compute  zero  mass  line  of  sight  guidance  law 
delta jheta,  delta_phi,  delta_x,  delta_y  :  float; 
Xjdot,  y_dot,  phLdot,  theta_dot :  float; 

debugtemp :  integer, 

procedure  initialize  Jos ;_gnc  is 
begin 

Il_receive(host,  angNavConst); 
ll_rcceive(host,  linNavConst); 

U_rcceive(host,  thrusterjnterval); 
ll_receive(host,  time_itep); 
fl_receive(host,  max_divert); 

ll_send(host,  angNavConst); 

H_scnd(host,  linNavConst); 

Il_send(host,  thmster_interval); 
ll_send(host,  time_step); 
ll_send(host,  max_diveit); 

thmster_time  :=  thrusterjnterval; 

end; 


- Main 

begin 


initialize  Jos_gnc; 
delta_theta  :=  0.0; 
delta_phi  :=  0.0; 
delta_x  :=  0.0; 
delta_y  :=  0.0; 
loop 

U_send(NET,  delta_theta); 
ll_send(NET,  delta_phi); 
ll_send(NET,  delta_x); 
U_send(NET,  delta_y); 

/*  debugtemp  :=  1; 

ll_send(host,debugtemp4sw);  *f 

ll_rcceive(NETl  x_dot); 
ll_receive(NFr,  y_dot); 
ll_receive(NET,  phi_dot); 
ll_receive(NET,  theta_dot); 

f*  debugtemp  :=  2; 

lljtend(host,debugtemp,lsw);  */ 
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if  thrusterjdme  >=  thrusterjinterval  then 
divert  :=  linNavConst  *  x_dot; 
if  (divert  >  max_divett)  then 

divert  :=  max_divert;  —  Limit  divert 

else 

if  (divert<  (-max_divert) )  then 
divert  :=  -max_divert; 
end  if; 
end  if; 

delta_x  :=  divert; 

divert  :=  linNavConst  *  y_dot; 
if  (divert>max_divert)  then 
divert  :=  max_divert; 

else 

if  (divert  <  (-max_divert)  )  then 
divert  :=  -max^divert; 
end  if; 
end  if; 

delta_y  :=  divert; 

delta_phi  :=  phi_dot  *  angNavConst; 
deltajtheta  .*=  theta_dot  *  angNavConst; 

thrusterjtime  :=  time_step; 

els& 

thrusterjtime  :=  thruster_time  +  time_step; 

delta_x  0.0; 
ddta_y  :=  0.0; 
delta_theta  :=  0.0; 
delta_phi  :=  0.0; 

end  if; 

end  loop; 
end; 
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